﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;

using System.Timers;
using System.Threading;
using Emgu.CV;
using Emgu.CV.CvEnum;
using Emgu.CV.Util;
using Emgu.CV.Features2D;
using Emgu.CV.Structure;
using Emgu.Util;
using DirectShowLib;
using System.IO.Ports;
using System.Xml;
using System.Runtime.InteropServices;

namespace VehicleTest
{
    public struct Video_Device
    {
        public string Device_Name;
        public int Device_ID;
        public Guid Identifier;

        public Video_Device(int ID, string Name, Guid Identity = new Guid())
        {
            Device_ID = ID;
            Device_Name = Name;
            Identifier = Identity;
        }

        public override string ToString()
        {
            return String.Format("[{0}] {1}: {2}", Device_ID, Device_Name, Identifier);
        }
    }

    public enum CamshiftShow
    {
        Camshift,
        Backproject,
        Histogram
    }

    public enum DetectionState
    {
        SIFT,
        SURF,
        USURF,
        CSIFT
    }

    public enum LoseState
    {
        DetectionLose,//Camshift和特征检测都运行,特征检测失去目标
        CamshiftLose,//Camshift和特征检测都运行,Camshift失去目标
        BothLose,//Camshift和特征检测都运行,都失去目标
        BothOK,//Camshift和特征检测都运行,都检测到目标
        OnlyCamshift_Lose,//Camshift运行,失去目标
        OnlyCamshift_OK,//Camshift运行,检测到目标
        OnlyDetection_Lose,//特征检测运行,失去目标
        OnlyDetection_OK,//特征检测运行,检测到目标
        BothClose //Camshift和特征检测都没运行
    }

    public enum DetectionTools
    {
        OnlyCamera,
        OnlyLMS,
        BothCameraAndLMS,
    }

    public struct VideoInfo
    {
        public String filename; 
        public int frameCount;
        public int width;
        public int height;
        public int currentFrame;
        public double fps;
    }

    public struct SIFTParameters
    {
        public int nFeatures;
        public int nOctaveLayers;
        public double contrastThreshold;
        public double edgeThreshold;
        public double sigma;
    }

    public partial class VehicleDetection : Form
    {
        public MainWindows mainwindows_detection;
        private VehicleDetectionZoom zoom;
        private DetectionRef detection_ref;
        public VehicleControl vehicle_control;
        public bool zoom_open = false;
        public bool ref_open = false;
        public bool control_open = false;
        private Capture capture;

        #region 读取视频相关参数
        private OpenFileDialog openfile = new OpenFileDialog();
        private VideoInfo video_info = new VideoInfo();
        private OpenFileDialog openfile_template = new OpenFileDialog();
        private System.Timers.Timer video_timer;
        private Thread video_thread;
        private uint frequency = 1;
        private int time1_sum = 0;
        private uint frame_count = 0;
        private bool videotest = false;
        private bool video_continue = false;
        private bool video_continue2 = true; //这是记录Camshift和SIFT的暂停
        private bool continue_flag = false;
        private bool thread_video_flag = false;
        
        private bool capture_start = false;
        private bool camshift_flag = false; //Camshift运行标志
        private bool detect_flag = false; //特征检测运行标志
        private Video_Device[] WebCams;

        private const int HEIGHT = 480;
        private const int WIDTH = 640;
        private Image<Bgr, Byte> frame;
        private Image<Bgr, Byte> frame_temp;
        private Image<Bgr, Byte> frame_rawshow;
        #endregion

        #region 录制视频相关参数
        private VideoWriter videoRecord_Original;
        private VideoWriter videoRecord_Camshift;
        private VideoWriter videoRecord_Detection;
        private bool record_pause = false;
        private bool recordOriginal_start = false;
        private bool recordCamshift_start = false;
        private bool recordDetection_start = false;
        private Image<Bgr, Byte> record_frame;
        private Image<Bgr, Byte> record_Camshift;
        private Image<Bgr, Byte> record_Detection;
        private bool canrecord = false;
        #endregion

        #region 标定参数
        private XmlDocument xml_readCalibrationParameters = new XmlDocument();
        private Matrix<double> intrinsic = new Matrix<double>(3, 3);
        private Matrix<double> distortion = new Matrix<double>(5, 1);
        private double alpha;
        private Matrix<double> rotation = new Matrix<double>(3, 3);
        private Matrix<double> deviation = new Matrix<double>(1, 3);
        private IntrinsicCameraParameters cali = new IntrinsicCameraParameters();
        private Matrix<float> map1;
        private Matrix<float> map2;
        private bool remap_flag = true; //为了只计算一次映射关系
        private bool cali_flag = false;
        #endregion

        #region 模版匹配
        private Image<Bgr, Byte> tpl;
        private Image<Bgr, Byte> match_frame;
        private Image<Gray, float> match_reference;
        private Image<Bgr, Byte> match_show;
        private Image<Gray, Byte> match_reference_show;
        private bool templatematch_flag = false;
        private double[] maxvalue;
        private double[] minvalue;
        private Point[] maxloc;
        private Point[] minloc;
        private Rectangle draw_window;
        #endregion

        #region Camshift
        private Rectangle selectwindow;
        private Rectangle mousewindow;
        private Point relate_down;
        private Point relate_up;
        private bool mouse_done = false;
        private bool down_done = false;
        private static RangeF range = new RangeF(0, 180);
        private DenseHistogram hist = new DenseHistogram(16, range);
        private Image<Bgr, Byte> histimg = null;
        private Image<Bgr, Byte> frame_camshift = null;
        private Image<Bgr, Byte> camshift_draw = null;
        private Image<Gray, byte> hue = null;
        private Image<Gray, byte> mask = null;
        private Image<Gray, byte> backproject = null;
        private Image<Bgr, byte> backproject_draw = null;
        private Image<Hsv, byte> hsv = null;
        private DenseHistogram hist_target = new DenseHistogram(16, range);  //检测到的目标的颜色直方图
        private Image<Bgr, Byte> histimg_target = null;  //检测到的目标的颜色直方图图像
        private Image<Bgr, Byte> histimg_target_show = null;
        private Image<Gray, byte> mask_hist = null; //为了计算检测目标颜色直方图而用到的掩码
        private Rectangle trackwin;
        private MCvConnectedComp trackcomp = new MCvConnectedComp();
        private MCvBox2D trackbox = new MCvBox2D();
        private Thread thread_camshift;
        private bool thread_camshift_flag = false;
        private bool cal_hist_flag = true;   //判断camshfit如何暂停的，以免暂停再开始时重新计算hist
        private volatile CamshiftShow showstate;
        private Image<Bgr, byte> camshift_show = null;
        private float hist_maxvalue;
        private float hist_minvalue;
        private float hist_target_maxvalue;
        private float hist_target_minvalue;
        private int[] maxinx;
        private int[] mininx;
        private int[] maxinx_target;
        private int[] mininx_target;
        public int Hmax_target = 180;
        public int Hmin_target = 0;
        public int Smax_target = 256;
        public int Smin_target = 30;
        public int Vmax_target = 256;
        public int Vmin_target = 10;
        public bool hist_target_show = false;  //显示目标直方图的标志
        public bool histdata_target_show = false; //显示目标直方图的数据
        public bool coeffdata_show = false; //显示目标直方图的相似度
        public bool data_show = false; //显示数据的总标志
        public HISTOGRAM_COMP_METHOD comp_method = HISTOGRAM_COMP_METHOD.CV_COMP_CORREL; //计算相似度的方法
        public bool flag_EMD = false;
        private double coeff = 0;
        public double EMD_threshold = 0.5;
        public double COEFF_threshold = 0.5;
        public double BHATTACHARYYA_threshold = 0.3;
        private PointF camshift_center = new PointF(0, 0);
        private float distance_center = 0;
        private int win_diatance_threshold = 50; //偏差阈值设置
        #endregion

        #region Detection
        private Thread thread_detect;
        private bool thread_detect_flag = false;
        private DetectionState detect_state;
        private SIFTParameters SIFTParams;
        private MCvSURFParams SurfParams;
        private MCvSURFParams USurfParams;
        private SIFTDetector sift;  //SIFT
        private SURFDetector surf; //SURF
        private SURFDetector usurf; //U-SURF
        private Image<Bgr, byte> tpl_draw;
        private Image<Bgr, byte> detect_target;
        private Image<Bgr, byte> target_draw;
        private Image<Bgr, byte> detect_draw;
        private Image<Bgr, byte> detect_show;
        private BruteForceMatcher<float> matcher;
        private HomographyMatrix homography = null;
        private VectorOfKeyPoint tplKeyPoints;
        private VectorOfKeyPoint targetKeyPoints;
        private Matrix<float> tplDescriptors;
        private Matrix<float> targetDescriptors;
        private Image<Gray, byte> detectionMask = null;
        private int k;//计算邻近描述点的个数
        private double uniquenessThreshold;//过滤匹配点的阈值
        private Matrix<int> indices;  //记录匹配之后的索引
        private Matrix<float> dist;   //记录索引中的距离
        private Matrix<byte> detect_mask = null; //过滤匹配点的掩码
        private bool matchrate = false;
        private bool detection_lose = true;
        private bool detection_lose1 = true;
        private bool detection_lose2 = true;
        private int Detection_threshold = 8;
        private float Shape_coeff = 0.6f; //判断特征检测目标的矩形框的形状系数
        private bool show_EffectivePoint = false;
        public bool mask_show = false;
        public bool mask_cancel = false;
        public double mask_threshold = 25;
        public int dilate_times = 5;
        public int erode_times = 5;
        public bool dilate_cancel = false;
        public bool erode_cancel = false;
        private XmlDocument xml_DetectionParameters = new XmlDocument();//数据保存到xml文件
        private XmlDocument xml_readDetectionParameters = new XmlDocument();
        private PointF detection_center = new PointF(0,0);
        private Rectangle trackwin_temp = new Rectangle(0,0,0,0);
        #endregion 

        #region 激光数据采集
        private SerialPort LMS111_serial = new SerialPort();
        public bool serial_receive_on = false;
        public double LMS_Resolution = 0.5;
        public double LMS_StartAngle = 30;
        public double LMS_EndAngle = 150;
        private int LMS_NUM = 241;
        private int[] LMS_data;  //存储激光距离信息
        private int[] LMS_data_temp;
        private int[] LMS_u;     //激光点在图像上的x坐标
        private int[] LMS_v;     //激光点在图像上的y坐标
        private int[] LMS_u_temp;   
        private int[] LMS_v_temp;
        private double[] LMS_X;     //激光点在水平平面XOZ'上的X坐标
        private double[] LMS_Z;     //激光点在水平平面XOZ'上的Z坐标
        private bool isLMSUsed = false;  //判断是否是用激光数据识别目标
        public double targetLine_Alpha = 0; //目标激光点拟合直线与X轴的夹角
        public double targetLine_Dis = 0;   //目标激光点拟合直线到原点的距离
        public double target_X = 0;     //目标点的坐标X,最终需要的数据
        public double target_Y = 1500;  //目标点的坐标Y,最终需要的数据
        public double distance = 0; //目标点的直线距离,以mm为单位
        public double angle = 0;    //偏转角度,以度为单位
        private double target_X_temp = 0;
        private double target_Y_temp = 0;
        private double angle_temp = 0;
        private double distance_temp = 0;
        private double K_threshold; //为了通过激光点判断目标,以度为单位
        public double LMS_distance_threshold = 2000; //为了判断目标距离是否是无效数据
        private int flag_firsttime = 10;
        private int target_index = 0; //目标点的下标
        private int target_index_min = 0;
        private int target_index_max = 0;
        private int index_threshold = 10; //摄像头和激光目标检测点阈值
        private bool LMSdataValid = false; //为了判断目标找到后激光数据是否在目标内
        public bool target_dataupdate_Camera = false;
        private DetectionTools detectoin_tools = DetectionTools.OnlyCamera;
        private double[] Pf = new double[3] { 0, 0, 0 };
        private double[] P_transfer = new double[3] { 0, 0, 0 };
        private double[] V_noise = new double[3] { 0, 0, 0 };
        private bool LMS111_done = false;
        private byte[] cmd_single = { 0x02, 0x73, 0x52, 0x4E, 0x20, 0x4C, 0x4D, 0x44, 0x73, 0x63, 0x61, 0x6E, 0x64, 0x61, 0x74, 0x61, 0x03 };
        private byte[] cmd_continue_start = { 0x02, 0x73, 0x45, 0x4E, 0x20, 0x4C, 0x4D, 0x44, 0x73, 0x63, 0x61, 0x6E, 0x64, 0x61, 0x74, 0x61, 0x20, 0x31, 0x03 };
        private byte[] cmd_continue_stop = { 0x02, 0x73, 0x45, 0x4E, 0x20, 0x4C, 0x4D, 0x44, 0x73, 0x63, 0x61, 0x6E, 0x64, 0x61, 0x74, 0x61, 0x20, 0x30, 0x03 };
        //用于摄像头检测
        private double distance_Cameratemp = 0;
        private double angle_Cameratemp = 0;
        private double target_X_Cameratemp = 0;
        private double target_Y_Cameratemp = 1500;
        private double target_CameraX_min = 640;
        private double target_CameraX_max = 0;
        private int target_Cameraindex = 0; //记录摄像头检测的目标
        private int target_Cameraindex_min = 0; 
        private int target_Cameraindex_max = 0;
        //用于激光测距仪检测
        private int target_LMSindex = 0; //记录激光测距仪检测的目标
        private int target_LMSindex_min = 0;
        private int target_LMSindex_max = 0;
        private double target_X_LMStemp = 0;
        private double target_Y_LMStemp = 0;
        private double angle_LMStemp = 0;
        private double distance_LMStemp = 0;
       
        #endregion

        #region 计时
        private DateTime start1;
        private DateTime end1;
        private TimeSpan time1;
        private DateTime start2;
        private DateTime end2;
        private TimeSpan time2;
        private DateTime start3;
        private DateTime end3;
        private TimeSpan time3;
        private DateTime start4;
        private DateTime end4;
        private TimeSpan time4;
        private DateTime start5;
        private DateTime end5;
        private TimeSpan time_detect;
        private DateTime start6;
        private DateTime end6;
        private TimeSpan time_match;
        private DateTime start7;
        private DateTime end7;
        private TimeSpan time_test;
        private bool time_flag = false;
        #endregion

        public VehicleDetection(MainWindows mw)
        {
            InitializeComponent();

            mainwindows_detection = mw;

            toolStripButton_Refresh.Enabled = false;
            toolStripButton_ReadCamera.Enabled = false;
            toolStripButton_ParameterSure.Enabled = false;
            button_TemplateMatchSatrt.Enabled = false;
            button_reference.Enabled = false;
            button_Continue.Enabled = false;
            button_CamshiftStart.Enabled = false;
            button_backproject.Enabled = false;
            button_Histogram.Enabled = false;
            button_DetectStart.Enabled = false;
            button_MatchRate.Enabled = false;
            button_StartRecord.Enabled = false;
            button_PauseRecord.Enabled = false;
            if (mainwindows_detection.serial_receive_on)
                toolStripButton_OpenControl.Enabled = true;
            else
                toolStripButton_OpenControl.Enabled = false;

            #region 读取的视频文件的设置
            openfile.Filter = "AVI文件(*.avi)|*.avi";  //设置文件类型
            openfile.Title = "选择测试的视频文件";     //显示对话框标题
            openfile.InitialDirectory = mainwindows_detection.path; //设置初始路径
            openfile.ValidateNames = true;  //文件有效性验证
            openfile.ShowHelp = true;
            openfile.HelpRequest += new EventHandler(openfile_HelpRequest);

            openfile_template.Filter = "BMP文件(*.bmp)|*.bmp";  //设置文件类型
            openfile_template.Title = "选择模版文件";     //显示对话框标题
            openfile_template.InitialDirectory = mainwindows_detection.path; //设置初始路径
            openfile_template.ValidateNames = true;  //文件有效性验证
            openfile_template.ShowHelp = true;
            openfile_template.HelpRequest += new EventHandler(openfile_HelpRequest);

            video_timer = new System.Timers.Timer();
            video_timer.Interval = 30;
            video_timer.Enabled = Enabled;
            video_timer.Stop();
            video_timer.Elapsed += new ElapsedEventHandler(Video_tick);
            #endregion

            #region 读取可用摄像头和串口
            //读取摄像头
            DsDevice[] _SystemCamereas = DsDevice.GetDevicesOfCat(FilterCategory.VideoInputDevice);
            WebCams = new Video_Device[_SystemCamereas.Length];
            for (int i = 0; i < _SystemCamereas.Length; i++)
            {
                WebCams[i] = new Video_Device(i, _SystemCamereas[i].Name, _SystemCamereas[i].ClassID); //fill web cam array
                toolStripComboBox_CameraSelect.Items.Add(WebCams[i].ToString());
            }
            if (toolStripComboBox_CameraSelect.Items.Count > 0)
            {
                toolStripComboBox_CameraSelect.SelectedIndex = 0; //Set the selected device the default
                toolStripButton_ReadCamera.Enabled = true; //Enable the start
            }
            else
            {
                toolStripButton_ReadCamera.Enabled = false;
            }

            //读取串口
            foreach (string s in SerialPort.GetPortNames())
            {
                toolStripComboBox_LMS111Serial.Items.Add(s);
            }
            if (toolStripComboBox_LMS111Serial.Items.Count > 0)
            {
                toolStripComboBox_LMS111Serial.SelectedIndex = 0;
                toolStripButton_ParameterSure.Enabled = true;
            }
            else
            {
                toolStripButton_ParameterSure.Enabled = false;
            }
            #endregion

            #region 标定参数和特征检测参数加载
            ParameterRead();
            cali.IntrinsicMatrix = intrinsic;
            cali.DistortionCoeffs = distortion;

            toolStripTextBox_Alpha.Text = alpha.ToString();
            toolStripTextBox_VnoiseX.Text = V_noise[0].ToString();
            toolStripTextBox_VnoiseY.Text = V_noise[1].ToString();
            toolStripTextBox_VnoiseZ.Text = V_noise[2].ToString();
            #endregion

            //原始图像显示
            frame = new Image<Bgr, Byte>(WIDTH, HEIGHT);
            frame_temp = new Image<Bgr, Byte>(WIDTH, HEIGHT);
            frame_rawshow = new Image<Bgr, Byte>(frame.Width/2, frame.Height/2);

            #region 模版匹配
            tpl = new Image<Bgr, Byte>(mainwindows_detection.path + "/doc/tpl.bmp");
            match_frame = new Image<Bgr, Byte>(frame.Width, frame.Height);
            match_show = new Image<Bgr, Byte>(frame.Width / 2, frame.Height / 2);
            match_reference_show = new Image<Gray, Byte>(frame.Width / 2, frame.Height / 2);
            draw_window = new Rectangle(0, 0, tpl.Width, tpl.Height);
            #endregion

            #region Camshift
            hue = new Image<Gray, byte>(frame.Width, frame.Height);
            mask = new Image<Gray, byte>(frame.Width, frame.Height);
            mask_hist = new Image<Gray, byte>(frame.Width, frame.Height);
            backproject = new Image<Gray, byte>(frame.Width, frame.Height);
            backproject_draw = new Image<Bgr, byte>(frame.Width, frame.Height);
            camshift_draw = new Image<Bgr, byte>(frame.Width, frame.Height);
            hsv = new Image<Hsv, byte>(frame.Width, frame.Height);
            histimg = new Image<Bgr, byte>(frame.Width, frame.Height);
            histimg_target = new Image<Bgr, byte>(frame.Width, frame.Height);
            histimg_target_show = new Image<Bgr, Byte>(frame.Width / 2, frame.Height / 2);
            frame_camshift = new Image<Bgr, Byte>(frame.Width, frame.Height);
            selectwindow = new Rectangle(0, 0, tpl.Width, tpl.Height);
            mousewindow = new Rectangle(0, 0, tpl.Width, tpl.Height);
            relate_down = new Point(0, 0);
            relate_up = new Point(0, 0);
            trackwin = new Rectangle(0, 0, tpl.Width, tpl.Height);
            showstate = CamshiftShow.Camshift;
            camshift_show = new Image<Bgr, Byte>(frame.Width / 2, frame.Height / 2);
            imageBox_Camshift.FunctionalMode = Emgu.CV.UI.ImageBox.FunctionalModeOption.Minimum;
            #endregion

            #region Detection
            //SIFT初始参数设置
            SIFTParams.nFeatures = 0;
            SIFTParams.nOctaveLayers = 3;
            SIFTParams.contrastThreshold = 0.04;
            SIFTParams.edgeThreshold = 10;
            SIFTParams.sigma = 1.6;
            sift = new SIFTDetector(SIFTParams.nFeatures, 
                                    SIFTParams.nOctaveLayers, 
                                    SIFTParams.contrastThreshold, 
                                    SIFTParams.edgeThreshold, 
                                    SIFTParams.sigma);

            //SURF初始参数设置
            SurfParams.Upright = false;
            SurfParams.HessianThreshold = 500;
            SurfParams.Extended = false;
            SurfParams.NOctaves = 4;
            SurfParams.NOctaveLayers = 2;
            surf = new SURFDetector(SurfParams);

            //U-SURF初始参数设置
            USurfParams.Upright = true;
            USurfParams.HessianThreshold = 500;
            USurfParams.Extended = false;
            USurfParams.NOctaves = 4;
            USurfParams.NOctaveLayers = 2;
            usurf = new SURFDetector(USurfParams);

            detect_state = DetectionState.SIFT;
            tpl_draw = new Image<Bgr, byte>(tpl.Width, tpl.Height);
            detect_target = new Image<Bgr, byte>(frame.Width, frame.Height);
            target_draw = new Image<Bgr, byte>(frame.Width, frame.Height);
            detect_draw = new Image<Bgr, byte>(tpl.Width + frame.Width, frame.Height);
            detect_show = new Image<Bgr, byte>(tpl.Width / 2 + frame.Width / 2, frame.Height / 2);
            detectionMask = new Image<Gray, byte>(frame.Width, frame.Height);
            detectionMask.SetValue(255);//将掩码初始化
            k = 2;  
            uniquenessThreshold = 0.75;

            numericUpDown_DetectionThreshold.Value = Detection_threshold;
            numericUpDown_ShapeCoeff.Value = (decimal)Shape_coeff;
            #endregion

            #region LMS111数据初始化
            LMS_data = new int[LMS_NUM];
            LMS_data_temp = new int[LMS_NUM];
            LMS_u = new int[LMS_NUM];
            LMS_u_temp = new int[LMS_NUM];
            LMS_v = new int[LMS_NUM];
            LMS_v_temp = new int[LMS_NUM];
            LMS_X = new double[LMS_NUM];
            LMS_Z = new double[LMS_NUM];

            #endregion

            //录像
            record_frame = new Image<Bgr, byte>(frame.Width, frame.Height);
            record_Camshift = new Image<Bgr, byte>(camshift_draw.Width, camshift_draw.Height);
            record_Detection = new Image<Bgr, Byte>(detect_draw.Width, detect_draw.Height);
        }

        #region 自定义函数
        private void Delay(int ms)
        {
            DateTime start = System.DateTime.Now;
            DateTime end;
            TimeSpan time;
            int delay = 0;
            while (delay < ms)
            {
                end = System.DateTime.Now;
                time = end - start;
                delay = time.Milliseconds;
            }
        }

        private void Init_TemplateRelated()
        {
            draw_window = new Rectangle(0, 0, tpl.Width, tpl.Height);
            selectwindow = new Rectangle(0, 0, tpl.Width, tpl.Height);
            mousewindow = new Rectangle(0, 0, tpl.Width, tpl.Height);
            trackwin = new Rectangle(0, 0, tpl.Width, tpl.Height);
            tpl_draw = new Image<Bgr, byte>(tpl.Width, tpl.Height);
            detect_draw = new Image<Bgr, byte>(tpl.Width + frame.Width, frame.Height);
            detect_show = new Image<Bgr, byte>(tpl.Width / 2 + frame.Width / 2, frame.Height / 2);
        }

        private void ParameterRead()
        {
            xml_readCalibrationParameters.Load(mainwindows_detection.path + "/doc/Calibration.xml");
            xml_readDetectionParameters.Load(mainwindows_detection.path + "/doc/DetectionParameters.xml");
            XmlNode currentNode;
            string[] temp;

            //标定参数加载
            currentNode = xml_readCalibrationParameters.SelectSingleNode("//intrinsics");
            string intrinsicText = currentNode.InnerText;
            temp = intrinsicText.Split(' ');
            intrinsic.Data[0, 0] = Convert.ToDouble(temp[0]);
            intrinsic.Data[0, 1] = Convert.ToDouble(temp[1]);
            intrinsic.Data[0, 2] = Convert.ToDouble(temp[2]);
            intrinsic.Data[1, 0] = Convert.ToDouble(temp[3]);
            intrinsic.Data[1, 1] = Convert.ToDouble(temp[4]);
            intrinsic.Data[1, 2] = Convert.ToDouble(temp[5]);
            intrinsic.Data[2, 0] = Convert.ToDouble(temp[6]);
            intrinsic.Data[2, 1] = Convert.ToDouble(temp[7]);
            intrinsic.Data[2, 2] = Convert.ToDouble(temp[8]);

            currentNode = xml_readCalibrationParameters.SelectSingleNode("//distortion");
            string distortionText = currentNode.InnerText;
            temp = distortionText.Split(' ');
            distortion.Data[0, 0] = Convert.ToDouble(temp[0]);
            distortion.Data[1, 0] = Convert.ToDouble(temp[1]);
            distortion.Data[2, 0] = Convert.ToDouble(temp[2]);
            distortion.Data[3, 0] = Convert.ToDouble(temp[3]);
            distortion.Data[4, 0] = Convert.ToDouble(temp[4]);

            currentNode = xml_readCalibrationParameters.SelectSingleNode("//alpha");
            string alphaText = currentNode.InnerText;
            alpha = Convert.ToDouble(alphaText);

            currentNode = xml_readCalibrationParameters.SelectSingleNode("//rotation");
            string rotationText = currentNode.InnerText;
            temp = rotationText.Split(' ');
            rotation.Data[0, 0] = Convert.ToDouble(temp[0]);
            rotation.Data[0, 1] = Convert.ToDouble(temp[1]);
            rotation.Data[0, 2] = Convert.ToDouble(temp[2]);
            rotation.Data[1, 0] = Convert.ToDouble(temp[3]);
            rotation.Data[1, 1] = Convert.ToDouble(temp[4]);
            rotation.Data[1, 2] = Convert.ToDouble(temp[5]);
            rotation.Data[2, 0] = Convert.ToDouble(temp[6]);
            rotation.Data[2, 1] = Convert.ToDouble(temp[7]);
            rotation.Data[2, 2] = Convert.ToDouble(temp[8]);

            currentNode = xml_readCalibrationParameters.SelectSingleNode("//deviation");
            string deviationText = currentNode.InnerText;
            temp = deviationText.Split(' ');
            deviation.Data[0, 0] = Convert.ToDouble(temp[0]);
            deviation.Data[0, 1] = Convert.ToDouble(temp[1]);
            deviation.Data[0, 2] = Convert.ToDouble(temp[2]);

            //加载特征检测的参数
            currentNode = xml_readDetectionParameters.SelectSingleNode("//CamshiftHSVThreshold");
            string camshiftHSVThresholdText = currentNode.InnerText;
            temp = camshiftHSVThresholdText.Split(' ');
            Hmin_target = Convert.ToInt32(temp[0]);
            Hmax_target = Convert.ToInt32(temp[1]);
            Smin_target = Convert.ToInt32(temp[2]);
            Smax_target = Convert.ToInt32(temp[3]);
            Vmin_target = Convert.ToInt32(temp[4]);
            Vmax_target = Convert.ToInt32(temp[5]);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//DetectionThreshold");
            string detectionThresholdText = currentNode.InnerText;
            Detection_threshold = Convert.ToInt32(detectionThresholdText);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//ShapeCoeff");
            string shapeCoeffText = currentNode.InnerText;
            Shape_coeff = Convert.ToSingle(shapeCoeffText);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//WinDistanceThreshold");
            string distanceThresholdText = currentNode.InnerText;
            win_diatance_threshold = Convert.ToInt32(distanceThresholdText);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//MaskThreshold");
            string maskThresholdText = currentNode.InnerText;
            mask_threshold = Convert.ToDouble(maskThresholdText);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//DilateTimes");
            string dilateTimesText = currentNode.InnerText;
            dilate_times = Convert.ToInt32(dilateTimesText);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//ErodeTimes");
            string erodeTimesText = currentNode.InnerText;
            erode_times = Convert.ToInt32(erodeTimesText);

            currentNode = xml_readDetectionParameters.SelectSingleNode("//LMSDistanceThreshold");
            string LMSDistanceThresholdText = currentNode.InnerText;
            LMS_distance_threshold = Convert.ToDouble(LMSDistanceThresholdText);
            toolStripTextBox_LMSDistanceThreshold.Text = LMSDistanceThresholdText;

            currentNode = xml_readDetectionParameters.SelectSingleNode("//KThreshold");
            string KThresholdText = currentNode.InnerText;
            K_threshold = Convert.ToDouble(KThresholdText);
            toolStripTextBox_KThreshold.Text = KThresholdText;


            currentNode = xml_readDetectionParameters.SelectSingleNode("//TargetIndexThreshold");
            string TargetIndexThresholdText = currentNode.InnerText;
            index_threshold = Convert.ToInt32(TargetIndexThresholdText);
            numericUpDown_TargetIndexDel.Value = index_threshold;
        }

        private void RecordInitial()
        {   
            videoRecord_Original = new VideoWriter("Original.avi", //文件名
                CvInvoke.CV_FOURCC('M', 'J', 'P', 'G'), //编码格式
                (int)numericUpDown_VideoFPS.Value, //帧率
                frame.Width, //视频宽度
                frame.Height, //视频高度
                true);//彩色
            recordOriginal_start = true;
           
            videoRecord_Camshift = new VideoWriter("Camshift.avi", //文件名
                CvInvoke.CV_FOURCC('M', 'J', 'P', 'G'), //编码格式
                (int)numericUpDown_VideoFPS.Value, //帧率
                camshift_draw.Width, //视频宽度
                camshift_draw.Height, //视频高度
                true);//彩色
            recordCamshift_start = true;
             
            videoRecord_Detection = new VideoWriter("Detectoin.avi", //文件名
                CvInvoke.CV_FOURCC('M', 'J', 'P', 'G'), //编码格式
                (int)numericUpDown_VideoFPS.Value, //帧率
                frame.Width + tpl.Width, //视频宽度
                frame.Height, //视频高度
                true);//彩色
            recordDetection_start = true;

            checkBox_VideoOriginal.Checked = true;
            checkBox_VideoCamshift.Checked = true;
            checkBox_VideoDetection.Checked = true;
        }

        private void LMSdateTransform()
        {
            double LMS_angle;
            double r_angle;
            double theta1;
            double theta2;

            double[] LMS_X_LaserPlan = new double[LMS_NUM];//激光点在激光平面坐标系上的X坐标
            double[] LMS_Y_LaserPlan = new double[LMS_NUM];//激光点在激光平面坐标系上的Y坐标
            double[] LMS_Z_LaserPlan = new double[LMS_NUM];//激光点在激光平面坐标系上的Z坐标

            for (int n = 0; n < LMS_NUM; n++)
            {
                LMS_angle = (n * LMS_Resolution + LMS_StartAngle) / 180.0 * Math.PI; //这是激光测距仪特定设置下的计算,要实现通用化,只要改变这个公式
                theta1 = Math.PI / 2 - LMS_angle;
                r_angle = Math.Asin(Math.Sin(alpha) * Math.Cos(theta1));
                theta2 = Math.Acos(Math.Cos(theta1) * Math.Cos(alpha) / Math.Cos(r_angle));//此时theta2一定为正,但是需要判断正负,为了求正确坐标
                if (n > (LMS_NUM / 2))
                {
                    theta2 = -theta2;
                }

                LMS_X[n] = LMS_data[n] * Math.Sin(theta2) * Math.Cos(r_angle);//用于定位的数据
                LMS_Z[n] = LMS_data[n] * Math.Cos(theta2) * Math.Cos(r_angle);//用于定位的数据

                LMS_X_LaserPlan[n] = LMS_data[n] / 1000.0 * Math.Sin(theta1);
                LMS_Y_LaserPlan[n] = -LMS_data[n] / 1000.0 * Math.Cos(theta1) * Math.Cos(alpha) * Math.Sin(alpha);
                LMS_Z_LaserPlan[n] = LMS_data[n] / 1000.0 * Math.Cos(theta1) * Math.Cos(alpha) * Math.Cos(alpha);

                Pf[0] = LMS_X_LaserPlan[n];
                Pf[1] = LMS_Y_LaserPlan[n];
                Pf[2] = LMS_Z_LaserPlan[n];

                double[] temp_LMS = new double[3];
                double[] temp_qs = new double[3];

                /***************************************************************
			    Pl = R * Pc + T  
			    其中 Pl为激光测距仪坐标系下点的坐标，在程序中为Pf[3]；
				 Pc为摄像头坐标系下点的坐标，在程序中为temp_qs[3]；
				 T 为激光测距仪坐标系下摄像头坐标原点的坐标，在程序中为V[3]；
				 R 为旋转矩阵，R_INV为逆矩阵
			    ****************************************************************/
                for (int i = 0; i < 3; i++)
                {
                    temp_LMS[i] = Pf[i] - deviation.Data[0, i] - V_noise[i];
                }

                for (int i = 0; i < 3; i++)
                {
                    temp_qs[i] = 0;
                    for (int j = 0; j < 3; j++)
                    {
                        temp_qs[i] += rotation.Data[i, j] * temp_LMS[j];
                    }
                }

                /*****************************************
			    p = K*Pc  摄像头坐标系中的点转换到图像平面
			    程序中K为Ins[3][3]矩阵, p为P_transfer[3]
			    *****************************************/
                for (int i = 0; i < 3; i++)
                {
                    P_transfer[i] = 0;
                    for (int j = 0; j < 3; j++)
                    {
                        P_transfer[i] += intrinsic.Data[i,j] * temp_qs[j];
                    }
                }
                for (int i = 0; i < 3; i++)
                {
                    P_transfer[i] = P_transfer[i] / P_transfer[2];
                }

                LMS_u[n] = (int)P_transfer[0];
                LMS_v[n] = (int)P_transfer[1];
            }
        }

        //
        // 输入参数：
        //     d1,d2,d3——相邻的三个激光点的距离信息
        //     beta    ——相邻两个激光点的夹角
        // 输出参数：
        //     theta——三个激光点所形成的三角形的顶角角度
        private double KDelCalcu(double d1, double d2, double d3, double beta)
        {
            double theta = 0;

            double xx = d1 * d1 + d3 * d3 - 2 * d1 * d3 * Math.Cos(2 * beta);
            double yy = d1 * d1 + d2 * d2 - 2 * d1 * d2 * Math.Cos(beta);
            double zz = d2 * d2 + d3 * d3 - 2 * d2 * d3 * Math.Cos(beta);
            double a = yy + zz - xx;
            theta = Math.Acos(a / (2 * Math.Sqrt(yy * zz)));

            theta = theta * 180 / Math.PI;
            return theta;
        }

        //
        // 输入参数：
        //     d1,d2——相邻两点激光距离
        //     beta ——相邻两点激光角度
        //     noise——激光测距仪测量噪声
        //     thrta——角度阈值
        //输出参数：
        //     result——判断是否为边界,
        private bool KJudgeIsEdge(double d1, double d2, double beta, double noise, double theta)
        {
            bool result = false;
            double del = Math.Abs(d1 - d2);
            double min = Math.Min(d1, d2);

            if (del < noise + min * 2 * Math.Tan(theta) * Math.Sin(0.5 * beta) / (Math.Cos(0.5 * beta) - Math.Sin(0.5 * beta) * Math.Tan(theta)))
                result = false;
            else
                result = true;

            return result;
        }

        private void LMSdataProcessingInControl(int min, int max, bool isLMS)
        {
            vehicle_control.chart_LMS111dataShow.Series[0].Points.Clear();
            vehicle_control.chart_LMS111dataShow.Series[1].Points.Clear();
            double[] LMS_dataX_temp = new double[LMS_NUM];
            double[] LMS_dataY_temp = new double[LMS_NUM];
            for (int i = 0; i < LMS_NUM; ++i)
            {
                LMS_dataX_temp[i] = LMS_X[i];
                LMS_dataY_temp[i] = LMS_Z[i];
            }
            double Ymax = LMS_dataY_temp.Max();
            double Xwide = Math.Max(Math.Abs(LMS_dataX_temp.Min()), Math.Abs(LMS_dataX_temp.Max()));
            vehicle_control.chart_LMS111dataShow.ChartAreas[0].AxisX.Maximum = Xwide * 1.1;
            vehicle_control.chart_LMS111dataShow.ChartAreas[0].AxisX.Minimum = -Xwide * 1.1;
            vehicle_control.chart_LMS111dataShow.ChartAreas[0].AxisY.Maximum = Ymax * 1.1;
            vehicle_control.chart_LMS111dataShow.ChartAreas[0].AxisY.Minimum = -10;

            for (int i = 0; i < LMS_NUM; ++i)
            {
                if (i >= min && i <= max)
                    vehicle_control.chart_LMS111dataShow.Series[1].Points.AddXY(LMS_dataX_temp[i], LMS_dataY_temp[i]);
                else
                    vehicle_control.chart_LMS111dataShow.Series[0].Points.AddXY(LMS_dataX_temp[i], LMS_dataY_temp[i]);
            }

            #region 最小二乘拟合
            if (max >= min)
            {
                #region 公式法
                //double a, b, c, d, theta1, theta2;
                //a = 0;
                //b = 0;
                //for (int i = min; i <= max; ++i)
                //{
                //    theta1 = (i * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                //    a += LMS_data_temp[i] * LMS_data_temp[i] * Math.Sin(2 * theta1);
                //    b += LMS_data_temp[i] * LMS_data_temp[i] * Math.Cos(2 * theta1);
                //}
                //c = 0;
                //d = 0;
                //for (int i = min; i <= max; ++i)
                //{
                //    theta1 = (i * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                //    for (int j = min; j <= max; ++j)
                //    {
                //        theta2 = (j * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                //        c += LMS_data_temp[i] * LMS_data_temp[j] * Math.Cos(theta1) * Math.Sin(theta2);
                //        d += LMS_data_temp[i] * LMS_data_temp[j] * Math.Cos(theta1 + theta2);
                //    }
                //}
                //int n = max - min + 1;
                //double y = a - 2 / n * c;
                //double x = b - d / n;
                //targetLine_Alpha = Math.Atan(y / x);
                //if (targetLine_Alpha < 0)
                //    targetLine_Alpha += Math.PI;
                //targetLine_Alpha *= 0.5;

                //double e = 0;
                //for (int i = min; i <= max; ++i)
                //{
                //    theta1 = (i * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                //    e += LMS_data_temp[i] * Math.Cos(theta1 - targetLine_Alpha);
                //}
                //targetLine_Dis = e / (max - min + 1);

                //double linePoint1X, linePoint1Y, linePoint2X, linePoint2Y;
                //double angle_start = (min * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                //double angle_end = (max * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                //linePoint1X = targetLine_Dis / (Math.Cos(targetLine_Alpha) + Math.Sin(targetLine_Alpha) * Math.Tan(angle_start));
                //linePoint1Y = linePoint1X * Math.Tan(angle_start);
                //linePoint2X = targetLine_Dis / (Math.Cos(targetLine_Alpha) + Math.Sin(targetLine_Alpha) * Math.Tan(angle_end));
                //linePoint2Y = linePoint2X * Math.Tan(angle_end);

                //vehicle_control.chart_LMS111dataShow.Series[2].Points.Clear();
                //vehicle_control.chart_LMS111dataShow.Series[2].Points.AddXY(linePoint1X, linePoint1Y);
                //vehicle_control.chart_LMS111dataShow.Series[2].Points.AddXY(linePoint2X, linePoint2Y);
                #endregion

                #region 求解广义逆法
                int N = max - min + 1;
                double[,] P = new double[N, 2];
                double[,] PINV = new double[2, N];
                double[] PD = new double[N];
                double[] input = new double[2 * N];

                for (int i = 0; i < N; ++i)
                {
                    P[i, 0] = LMS_dataX_temp[i + min];
                    P[i, 1] = 1;
                    PD[i] = LMS_dataY_temp[i + min];
                }
                for(int i=0; i<N; ++i)
                    for (int j = 0; j < 2; ++j)
                    {
                        input[i * 2 + j] = P[i, j];
                    }

                PINV = MYCAL(N, 2, input, 0.001);

                double k, b;
                k = 0;
                b = 0;
                for (int i = 0; i < N; ++i)
                {
                    k += PINV[0, i] * PD[i];
                    b += PINV[1, i] * PD[i];
                }

                double linePoint1X, linePoint1Y, linePoint2X, linePoint2Y;
                double angle_start = (min * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                double angle_end = (max * LMS_Resolution + LMS_StartAngle) / 180 * Math.PI;
                linePoint1X = b / (Math.Tan(angle_start) - k);
                linePoint1Y = linePoint1X * Math.Tan(angle_start);
                linePoint2X = b / (Math.Tan(angle_end) - k);
                linePoint2Y = linePoint2X * Math.Tan(angle_end);

                //绘制拟合直线
                vehicle_control.chart_LMS111dataShow.Series[2].Points.Clear();
                vehicle_control.chart_LMS111dataShow.Series[2].Points.AddXY(linePoint1X, linePoint1Y);
                vehicle_control.chart_LMS111dataShow.Series[2].Points.AddXY(linePoint2X, linePoint2Y);

                if (isLMS)
                {
                    target_X_temp = (linePoint1Y + linePoint2Y) / 2;
                    target_Y_temp = -(linePoint1X + linePoint2X) / 2; //转化为全局坐标系的坐标
                }
                #endregion
            }
            #endregion
            //绘制目标点
            vehicle_control.chart_LMS111dataShow.Series[3].Points.Clear();
            vehicle_control.chart_LMS111dataShow.Series[3].Points.AddXY(-target_Y_temp, target_X_temp); //target的X和Y是全局坐标系的X和Y,分别与激光测距仪的Z和-X对应
        }

        [DllImport("mydll.dll", CallingConvention = CallingConvention.Cdecl)]
        static extern unsafe double** mycal(int m, int n, double* data, double wucha);//求解广义逆

        private unsafe double[,] MYCAL(int m, int n, double[] data, double wucha)
        {
            double[,] result = new double[n, m];
            double** result2;
            double* data2 = stackalloc double[m * n];
            for (int i = 0; i < m * n; ++i)
            {
                data2[i] = data[i];
            }
            result2 = mycal(m, n, data2, wucha);
            for(int i=0; i<n; ++i)
                for (int j = 0; j < m; ++j)
                {
                    result[i, j] = result2[i][j];
                }
            return result;
        }

        private Bgr HSV2BGR(Hsv color_hsv)
        {
            Bgr color_bgr = new Bgr(0, 0, 0);
            double h = color_hsv.Hue * 2;
            double s = color_hsv.Satuation / 255;
            double v = color_hsv.Value / 255;
            double R = 0, G = 0, B = 0;
            int i;

            if (h >= 360)
                h -= 360;

            if (s == 0)
            {
                R = v;
                G = v;
                B = v;
            }
            else
            {
                h = h / 60;
                i = (int)Math.Floor(h);
                double f = h - i;
                double a = v * (1 - s);
                double b = v * (1 - s * f);
                double c = v * (1 - s * (1 - f));

                switch (i)
                {
                    case 0:
                        R = v;
                        G = c;
                        B = a;
                        break;
                    case 1:
                        R = b;
                        G = v;
                        B = a;
                        break;
                    case 2:
                        R = a;
                        G = v;
                        B = c;
                        break;
                    case 3:
                        R = a;
                        G = b;
                        B = v;
                        break;
                    case 4:
                        R = c;
                        G = a;
                        B = v;
                        break;
                    case 5:
                        R = v;
                        G = a;
                        B = b;
                        break;
                    default:
                        break;
                }
            }

            R *= 255;
            G *= 255;
            B *= 255;

            color_bgr.Red = (int)R;
            color_bgr.Green = (int)G;
            color_bgr.Blue = (int)B;

            return color_bgr;
        }

        private void openfile_HelpRequest(object sender, EventArgs e)
        {

        }

        //判断目标是否丢失
        private LoseState TargetLose()
        {
            bool result_camshift = true;
            bool result_detection = true;
            LoseState result = LoseState.BothLose;

            if (flag_EMD)  
            {
                //陆地移动距离的阈值
                if (coeff < EMD_threshold)
                    result_camshift = false;
                else
                    result_camshift = true;
            }
            else if (comp_method == HISTOGRAM_COMP_METHOD.CV_COMP_CORREL)
            {
                //直方图比较相关法的阈值
                if (coeff > COEFF_threshold)
                    result_camshift = false;
                else
                    result_camshift = true;
            }
            else if (comp_method == HISTOGRAM_COMP_METHOD.CV_COMP_BHATTACHARYYA)
            {
                //直方图比较BHATTACHARYYA距离的阈值
                if (coeff < BHATTACHARYYA_threshold)
                    result_camshift = false;
                else
                    result_camshift = true;
            }

            result_detection = detection_lose;

            if (detect_flag && camshift_flag)//Camshfit和特征检测都运行
            {
                if (result_camshift && result_detection)
                {
                    result = LoseState.BothLose;  //Camshift和Detection都失去目标
                }
                else if (result_camshift && (!result_detection))
                {
                    result = LoseState.CamshiftLose;  //Camshift失去目标
                }
                else if (result_detection && (!result_camshift))
                {
                    result = LoseState.DetectionLose;
                }
                else
                {
                    result = LoseState.BothOK;
                }
            }
            else if (camshift_flag && (!detect_flag))//只运行了Camshift
            {
                if (result_camshift)
                    result = LoseState.OnlyCamshift_Lose;
                else
                    result = LoseState.OnlyCamshift_OK;
            }
            else if ((!camshift_flag) && detect_flag)//只运行了Camshift
            {
                if (result_detection)
                    result = LoseState.OnlyDetection_Lose;
                else
                    result = LoseState.OnlyDetection_OK;
            }
            else//都没运行
            {
                result = LoseState.BothClose;
            }
           

            return result;
        }

        //判断四个点组成的矩形合不合理
        private bool RectJudge(PointF[] p)
        {
            bool result = false;

            if ((p[0].X < p[1].X) && (p[0].X < p[2].X) && (p[3].X < p[1].X) && (p[3].X < p[2].X)
                && (p[0].Y > p[3].Y) && (p[0].Y > p[2].Y) && (p[1].Y > p[3].Y) && (p[1].Y > p[2].Y))//判断相对位置
            {
                float width_u = p[2].X - p[3].X; //矩形的上宽
                float width_d = p[1].X - p[0].X; //矩形的下宽
                float height_l = p[0].Y - p[3].Y; //矩形的左高
                float height_r = p[1].Y - p[2].Y; //矩形的右高
                float width_coeff = (width_d > width_u) ? (width_u / width_d) : (width_d / width_u);
                float height_coeff = (height_r > height_l) ? (height_l / height_r) : (height_r / height_l);
                if ((width_coeff > Shape_coeff) && (height_coeff > Shape_coeff))
                    result = true;
                else
                    result = false;
            }
            else
            {
                result = false;
            }
            return result;
        }

        //根据特征匹配结果构造Camshift的跟踪框
        private Rectangle GenerateCamshifWin(PointF[] p)
        {
            Rectangle win;

            if (Math.Abs(p[0].X - p[2].X) * Math.Abs(p[0].Y - p[2].Y) > Math.Abs(p[1].X - p[3].X) * Math.Abs(p[1].Y - p[3].Y))
            {
                win = new Rectangle((int)p[3].X, (int)p[3].Y, (int)Math.Abs(p[1].X - p[3].X), (int)Math.Abs(p[1].Y - p[3].Y));
            }
            else
            {
                win = new Rectangle((int)p[0].X, (int)p[2].Y, (int)Math.Abs(p[0].X - p[2].X), (int)Math.Abs(p[0].Y - p[2].Y));
            }
            return win;
        }

        //求出矩形对角线的角点
        private PointF CalculateCross(PointF[] p)
        {
            PointF cross = new PointF(0,0);
            float x3_1 = p[3].X - p[1].X;
            float y2_0 = p[2].Y - p[0].Y;
            float x2_0 = p[2].X - p[0].X;
            float y3_1 = p[3].Y - p[1].Y;
            float a = x3_1 * y2_0;
            float b = x2_0 * y3_1;

            cross.X = (a * p[0].X - b * p[1].X + x3_1 * x2_0 * (p[1].Y - p[0].Y)) / (a - b);
            cross.Y = (a * p[1].Y - b * p[0].Y + y2_0 * y3_1 * (p[0].X - p[1].X)) / (a - b);
            return cross;
        }

        //计算两点直接的距离
        private float CalculateDistance(PointF p1, PointF p2)
        {
            float d = 0;
            d = (float)Math.Sqrt((p1.X - p2.X) * (p1.X - p2.X) + (p1.Y - p2.Y) * (p1.Y - p2.Y));
            return d;
        }

        public void PreClosing()
        {
            if (capture_start)
            {
                capture.Stop();
                capture.Dispose();
            }

            if (thread_camshift_flag)
                thread_camshift.Abort();
            if (thread_detect_flag)
                thread_detect.Abort();
            if (thread_video_flag)
                video_thread.Abort();

            if (LMS111_serial.IsOpen)
                LMS111_serial.Close();

            if (zoom_open)
            {
                if (!zoom.IsDisposed)
                    zoom.Dispose();
            }

            if (ref_open)
            {
                if (!detection_ref.IsDisposed)
                    detection_ref.Dispose();
            }

            if (control_open)
            {
                if (!vehicle_control.IsDisposed)
                    vehicle_control.Dispose();
            }

        }
        // Ensures the disposing of the capture device
        protected override void OnFormClosing(FormClosingEventArgs e)
        {
            PreClosing();
            
            base.OnFormClosing(e);   //在MSDN中规定如果重载了OnFormClosing函数，最后要调用基类的OnFormClosing函数
        }
        #endregion

        #region 控件事件
        private void toolStripButton_OpenFile_Click(object sender, EventArgs e)
        {
            if (openfile.ShowDialog() == DialogResult.OK)
            {
                video_info.filename = openfile.FileName;
                videotest = true;
                continue_flag = false; //重置读取视频的状态
                frame_count = 0; //计数器清零
                toolStripButton_ParameterSure.Enabled = false;
                toolStripButton_ReadCamera.Enabled = true;
                
                //显示
                richTextBox_Show.SelectionColor = Color.Green;
                richTextBox_Show.AppendText("选取了视频文件：" + video_info.filename + "\n");
                richTextBox_Show.ScrollToCaret();
            }
        }

        private void toolStripButton_ReadCamera_Click(object sender, EventArgs e)
        {
            if (capture == null)
            {
                try
                {
                    if (videotest)
                    {
                        capture = new Capture(video_info.filename);
                        video_info.fps = CvInvoke.cvGetCaptureProperty(capture, CAP_PROP.CV_CAP_PROP_FPS);
                        video_info.frameCount = (int)CvInvoke.cvGetCaptureProperty(capture, CAP_PROP.CV_CAP_PROP_FRAME_COUNT);
                        video_info.width = (int)CvInvoke.cvGetCaptureProperty(capture, CAP_PROP.CV_CAP_PROP_FRAME_WIDTH);
                        video_info.height = (int)CvInvoke.cvGetCaptureProperty(capture, CAP_PROP.CV_CAP_PROP_FRAME_HEIGHT);

                        video_timer.Interval = (int)(1000 / video_info.fps);
                    }
                    else
                    {
                        capture = new Capture(toolStripComboBox_CameraSelect.SelectedIndex);
                        capture.ImageGrabbed += new Emgu.CV.Capture.GrabEventHandler(capture_ImageGrabbed);
                    }
                    
                }
                catch (NullReferenceException except)
                {
                    MessageBox.Show(except.Message);
                }
            }

            if (capture != null)
            {
                if (toolStripButton_ReadCamera.Text == "采集图像")
                {
                    if (videotest)
                    {
                        //if (continue_flag)
                        //    video_continue = true;
                        //else
                        //{
                        //    frame_temp = capture.QueryFrame();   //这边为了在模版匹配的时候停留在第一帧
                        //    frame_count++;
                        //}
                        //video_timer.Start();

                        if (continue_flag)
                        {
                            video_continue = true;
                            video_thread = new Thread(VideoThread);
                            video_thread.Start();
                            thread_video_flag = true;
                        }
                        else
                        {
                            thread_video_flag = true;
                            frame_temp = capture.QueryFrame();   //这边为了在模版匹配的时候停留在第一帧
                            frame_count++; 
                            video_continue = true;
                            video_thread = new Thread(VideoThread);
                            video_thread.Start();
                        }                        

                        richTextBox_Show.SelectionColor = Color.Green;
                        richTextBox_Show.AppendText("打开了视频文件！\n");
                        richTextBox_Show.ScrollToCaret();
                    }
                    else
                    {
                        capture.Start();

                        richTextBox_Show.SelectionColor = Color.Green;
                        richTextBox_Show.AppendText("Have chosen camera [" + toolStripComboBox_CameraSelect.SelectedIndex.ToString() + "]!\n");
                        richTextBox_Show.ScrollToCaret();
                    }

                    templatematch_flag = true;
                    button_TemplateMatchSatrt.Enabled = true;

                    capture_start = true;
                    toolStripButton_ReadCamera.Text = "停止采集";

                    toolStripComboBox_CameraSelect.Enabled = false;
                    toolStripButton_Refresh.Enabled = false;
                    toolStripButton_OpenFile.Enabled = false;
                }
                else
                {
                    if (videotest)
                    {
                        video_continue = false;
                        richTextBox_Show.SelectionColor = Color.Red;
                        richTextBox_Show.AppendText("暂停了视频文件!\n");
                        richTextBox_Show.ScrollToCaret();
                    }
                    else
                    {
                        capture.Stop();
                        richTextBox_Show.SelectionColor = Color.Red;
                        richTextBox_Show.AppendText("Have closed camera [" + toolStripComboBox_CameraSelect.SelectedIndex.ToString() + "]!\n");
                        richTextBox_Show.ScrollToCaret();
                    }

                    cal_hist_flag = true;   //在此处暂停，重新开始后要重新计算hist
                    capture_start = false;
                    toolStripButton_ReadCamera.Text = "采集图像";

                    toolStripComboBox_CameraSelect.Enabled = false;
                    toolStripButton_Refresh.Enabled = true;

                    button_CamshiftStart.Enabled = false;
                    button_backproject.Enabled = false;
                    button_Histogram.Enabled = false;
                    button_DetectStart.Enabled = false;
                    button_MatchRate.Enabled = false;
                }
            }
        }

        private void toolStripComboBox_CameraSelect_DropDown(object sender, EventArgs e)
        {
            //读取摄像头
            DsDevice[] _SystemCamereas = DsDevice.GetDevicesOfCat(FilterCategory.VideoInputDevice);

            toolStripComboBox_CameraSelect.Items.Clear();
            WebCams = new Video_Device[_SystemCamereas.Length];
            for (int i = 0; i < _SystemCamereas.Length; i++)
            {
                WebCams[i] = new Video_Device(i, _SystemCamereas[i].Name, _SystemCamereas[i].ClassID); //fill web cam array
                toolStripComboBox_CameraSelect.Items.Add(WebCams[i].ToString());
            }

            videotest = false;  //使用摄像头采集
            toolStripButton_ParameterSure.Enabled = true;
        }

        private void toolStripComboBox_LMS111Serial_DropDown(object sender, EventArgs e)
        {
            toolStripComboBox_LMS111Serial.Items.Clear();
            foreach (string s in SerialPort.GetPortNames())
            {
                toolStripComboBox_LMS111Serial.Items.Add(s);
            }
        }

        private void toolStripButton_Refresh_Click(object sender, EventArgs e)
        {
            toolStripComboBox_CameraSelect.Enabled = true;
            toolStripButton_OpenFile.Enabled = true;
            toolStripButton_ParameterSure.Enabled = true;
            button_ChangeTemplate.Enabled = true;
            toolStripButton_Calibration.Enabled = true;

            button_TemplateMatchSatrt.Enabled = false;
            button_reference.Enabled = false;
            button_Continue.Enabled = false;

            imageBox_rawvideo.Image = new Image<Bgr, Byte>(mainwindows_detection.path + "/image/camerabackground.jpg");
            imageBox_TemplateMatch.Image = new Image<Bgr, Byte>(mainwindows_detection.path + "/image/camerabackground.jpg");
            imageBox_Camshift.Image = new Image<Bgr, Byte>(mainwindows_detection.path + "/image/camerabackground.jpg");
            imageBox_Detection.Image = new Image<Bgr, Byte>(mainwindows_detection.path + "/image/camerabackground4.jpg");
            imageBox_template.Image = new Image<Bgr, Byte>(mainwindows_detection.path + "/image/camerabackground3.jpg");

            if (videotest)
            {
                //    video_timer.Stop();
                video_continue = false;
                if (thread_video_flag)
                    video_thread.Abort();
            }

            if (capture != null)
            {
                capture.Dispose();
                capture = null;
            }
            
            richTextBox_Show.SelectionColor = Color.Red;
            richTextBox_Show.AppendText("释放了内存，重新选择摄像头或视频文件！\n");
            richTextBox_Show.ScrollToCaret();
        }

        private void toolStripButton_ParameterSure_Click(object sender, EventArgs e)
        {
            try
            {
                if (LMS111_serial.IsOpen)
                {
                    serial_receive_on = false;
                    LMS111_serial.DataReceived -= new SerialDataReceivedEventHandler(event_LMS111ReadSerialport);
                    Delay(200);  //很重要,如果不延迟,因为多线程的关系会出错
                    LMS111_serial.DiscardInBuffer();
                    LMS111_serial.DiscardOutBuffer();
                    LMS111_serial.Close();
                    LMS111_serial.DataReceived -= new SerialDataReceivedEventHandler(event_LMS111ReadSerialport);  

                    richTextBox_Show.SelectionColor = Color.Red;
                    richTextBox_Show.AppendText("LMS111 have closed: " + toolStripComboBox_LMS111Serial.Text + "!\n");
                    richTextBox_Show.ScrollToCaret();
                    toolStripButton_ParameterSure.Text = "打开LMS111";
                    toolStripComboBox_LMS111Serial.Enabled = true;
                }
                else
                {
                    LMS111_serial.PortName = toolStripComboBox_LMS111Serial.Text;
                    LMS111_serial.BaudRate = 115200;
                    LMS111_serial.DataBits = 8;
                    LMS111_serial.StopBits = StopBits.One;
                    LMS111_serial.Parity = Parity.None;

                    mainwindows_detection.receive_state = ReceiveState.VehiclePacket;
                    LMS111_serial.Open();
                    LMS111_serial.DiscardInBuffer();
                    LMS111_serial.DiscardOutBuffer();
                    LMS111_serial.DataReceived += new SerialDataReceivedEventHandler(event_LMS111ReadSerialport);          
       
                    serial_receive_on = true;
                    LMS111_done = true;
                    richTextBox_Show.SelectionColor = Color.Green;
                    richTextBox_Show.AppendText("LMS111 have chosen: " + toolStripComboBox_LMS111Serial.Text + "!\n");
                    richTextBox_Show.ScrollToCaret();
                    toolStripButton_ParameterSure.Text = "关闭LMS111";
                    toolStripComboBox_LMS111Serial.Enabled = false;
                }
            }
            catch (System.UnauthorizedAccessException ex)
            {
                MessageBox.Show("串口冲突！重新选择可用串口:\n" + ex.Message);
            }
        }

        private void toolStripButtonTimeShow_Click(object sender, EventArgs e)
        {
            if (toolStripButton_TimeShow.Text == "打开计时")
            {
                //计时打开
                toolStripButton_TimeShow.Text = "关闭计时";
                time_flag = true;
            }
            else
            {
                //计时关闭
                toolStripButton_TimeShow.Text = "打开计时";
                time_flag = false;
            }
        }

        private void toolStripButton_Calibration_Click(object sender, EventArgs e)
        {
            templatematch_flag = true;

            if (!cali_flag)
            {
                if (capture_start)
                {
                    toolStripButton_Calibration.Text = "放弃校正";
                    cali_flag = true;
                    remap_flag = true;
                    
                    //button_TemplateMatchSatrt.Enabled = true;
                    //button_reference.Enabled = true;
                    //button_Continue.Enabled = true;
                }
                else
                {
                    richTextBox_Show.SelectionColor = Color.Red;
                    richTextBox_Show.AppendText("警告：摄像头未打开,无法进行校正！\n");
                    richTextBox_Show.ScrollToCaret();
                }
            }
            else
            {
                toolStripButton_Calibration.Text = "图像校正";
                cali_flag = false;

                //button_TemplateMatchSatrt.Enabled = false;
                //button_reference.Enabled = false;
                //button_Continue.Enabled = false;
                //button_CamshiftStart.Enabled = false;
                //button_backproject.Enabled = false;
                //button_Histogram.Enabled = false;
                //button_DetectStart.Enabled = false;
                //button_MatchRate.Enabled = false;
            }

        }

        private void toolStripButton_ZoomOut_Click(object sender, EventArgs e)
        {
            if (toolStripButton_ZoomOut.Text == "放大显示")
            {
                if ((zoom == null) || (zoom.IsDisposed))
                {
                    zoom = new VehicleDetectionZoom(this);
                }

                zoom.Show();

                toolStripButton_ZoomOut.Text = "关闭显示";
                zoom_open = true;
            }
            else
            {
                toolStripButton_ZoomOut.Text = "放大显示";
                zoom_open = false;
                zoom.Hide();
            }
            
        }

        private void toolStripButton_Ref_Click(object sender, EventArgs e)
        {

            if (toolStripButton_Ref.Text == "打开参考")
            {
                if ((detection_ref == null) || (detection_ref.IsDisposed))
                {
                    detection_ref = new DetectionRef(this);
                }

                detection_ref.Show();

                toolStripButton_Ref.Text = "关闭参考";
                ref_open = true;
            }
            else
            {
                toolStripButton_Ref.Text = "打开参考";
                ref_open = false;
                detection_ref.Hide();
            }
        }

        private void toolStripTextBox_KThreshold_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(toolStripTextBox_LMSDistanceThreshold.Text);
                test = Convert.ToDouble(toolStripTextBox_KThreshold.Text);
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void toolStripTextBox_LMSDistanceThreshold_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double test;
                test = Convert.ToDouble(toolStripTextBox_LMSDistanceThreshold.Text);
                test = Convert.ToDouble(toolStripTextBox_KThreshold.Text);
                toolStripButton_SaveParameters.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_SaveParameters.Enabled = false;
            }
        }

        private void toolStripTextBox_Alpha_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double x = Convert.ToDouble(toolStripTextBox_VnoiseX.Text);
                double y = Convert.ToDouble(toolStripTextBox_VnoiseY.Text);
                double z = Convert.ToDouble(toolStripTextBox_VnoiseZ.Text);
                double a = Convert.ToDouble(toolStripTextBox_Alpha.Text);

                toolStripButton_LMSCaliSure.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_LMSCaliSure.Enabled = false;
            }
        }

        private void toolStripTextBox_VnoiseX_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double x = Convert.ToDouble(toolStripTextBox_VnoiseX.Text);
                double y = Convert.ToDouble(toolStripTextBox_VnoiseY.Text);
                double z = Convert.ToDouble(toolStripTextBox_VnoiseZ.Text);
                double a = Convert.ToDouble(toolStripTextBox_Alpha.Text);

                toolStripButton_LMSCaliSure.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_LMSCaliSure.Enabled = false;
            }
        }

        private void toolStripTextBox_VnoiseY_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double x = Convert.ToDouble(toolStripTextBox_VnoiseX.Text);
                double y = Convert.ToDouble(toolStripTextBox_VnoiseY.Text);
                double z = Convert.ToDouble(toolStripTextBox_VnoiseZ.Text);
                double a = Convert.ToDouble(toolStripTextBox_Alpha.Text);

                toolStripButton_LMSCaliSure.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_LMSCaliSure.Enabled = false;
            }
        }

        private void toolStripTextBox_VnoiseZ_TextChanged(object sender, EventArgs e)
        {
            try
            {
                double x = Convert.ToDouble(toolStripTextBox_VnoiseX.Text);
                double y = Convert.ToDouble(toolStripTextBox_VnoiseY.Text);
                double z = Convert.ToDouble(toolStripTextBox_VnoiseZ.Text);
                double a = Convert.ToDouble(toolStripTextBox_Alpha.Text);

                toolStripButton_LMSCaliSure.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_LMSCaliSure.Enabled = false;
            }
        }

        private void toolStripTextBox_Alpha_Click(object sender, EventArgs e)
        {
            try
            {
                double x = Convert.ToDouble(toolStripTextBox_VnoiseX.Text);
                double y = Convert.ToDouble(toolStripTextBox_VnoiseY.Text);
                double z = Convert.ToDouble(toolStripTextBox_VnoiseZ.Text);
                double a = Convert.ToDouble(toolStripTextBox_Alpha.Text);

                toolStripButton_LMSCaliSure.Enabled = true;
            }
            catch (FormatException)
            {
                toolStripButton_LMSCaliSure.Enabled = false;
            }
        }

        private void toolStripButton_LMSCaliSure_Click(object sender, EventArgs e)
        {
            V_noise[0] = Convert.ToDouble(toolStripTextBox_VnoiseX.Text);
            V_noise[1] = Convert.ToDouble(toolStripTextBox_VnoiseY.Text);
            V_noise[2] = Convert.ToDouble(toolStripTextBox_VnoiseZ.Text);
            alpha = Convert.ToDouble(toolStripTextBox_Alpha.Text);

            LMS_distance_threshold = Convert.ToDouble(toolStripTextBox_LMSDistanceThreshold.Text);
            K_threshold = Convert.ToDouble(toolStripTextBox_KThreshold.Text);
        }

        private void toolStripButton_OpenControl_Click(object sender, EventArgs e)
        {
            if (toolStripButton_OpenControl.Text == "打开控制")
            {
                if ((vehicle_control == null) || (vehicle_control.IsDisposed))
                {
                    vehicle_control = new VehicleControl(this);
                }

                vehicle_control.Show();
                toolStripButton_OpenControl.Text = "关闭控制";

                control_open = true;
            }
            else
            {
                control_open = false;

                toolStripButton_OpenControl.Text = "打开控制";
                vehicle_control.Hide();
            }
            
        }

        private void toolStripButton_SaveParameters_Click(object sender, EventArgs e)
        {
            xml_DetectionParameters.Load(mainwindows_detection.path + "/doc/ParametersModel.xml");
            XmlElement xml_root = xml_DetectionParameters.DocumentElement;
            //增加节点
            XmlElement newDetectionThreshold = xml_DetectionParameters.CreateElement("DetectionThreshold");
            XmlElement newShapeCoeff = xml_DetectionParameters.CreateElement("ShapeCoeff");
            XmlElement newDistanceThreshold = xml_DetectionParameters.CreateElement("WinDistanceThreshold");
            XmlElement newMaskThreshold = xml_DetectionParameters.CreateElement("MaskThreshold");
            XmlElement newDilateTimes = xml_DetectionParameters.CreateElement("DilateTimes");
            XmlElement newErodeTimes = xml_DetectionParameters.CreateElement("ErodeTimes");
            XmlElement newCamshiftHSVThreshold = xml_DetectionParameters.CreateElement("CamshiftHSVThreshold");
            XmlElement newLMSDistanceThreshold = xml_DetectionParameters.CreateElement("LMSDistanceThreshold");
            XmlElement newKThreshold = xml_DetectionParameters.CreateElement("KThreshold");
            XmlElement newTargetIndexThreshold = xml_DetectionParameters.CreateElement("TargetIndexThreshold");
            //元素值
            string detectionThreshold_data = Detection_threshold.ToString();
            string shapeCoeff_data = Shape_coeff.ToString();
            string distanceThreshold_data = win_diatance_threshold.ToString();
            string maskThreshold_data = mask_threshold.ToString();
            string dilateTimes_data = dilate_times.ToString();
            string erodeTimes_data = erode_times.ToString();
            string camshiftHSVThreshold_data = Hmin_target.ToString() + " " + Hmax_target.ToString() + " " +
                                               Smin_target.ToString() + " " + Smax_target.ToString() + " " +
                                               Vmin_target.ToString() + " " + Vmax_target.ToString();
            XmlText detectionThresholdText = xml_DetectionParameters.CreateTextNode(detectionThreshold_data);
            XmlText shapeCoeffText = xml_DetectionParameters.CreateTextNode(shapeCoeff_data);
            XmlText diatanceThresholdText = xml_DetectionParameters.CreateTextNode(distanceThreshold_data);
            XmlText maskThresholdText = xml_DetectionParameters.CreateTextNode(maskThreshold_data);
            XmlText dilateTimesText = xml_DetectionParameters.CreateTextNode(dilateTimes_data);
            XmlText erodeTimesText = xml_DetectionParameters.CreateTextNode(erodeTimes_data);
            XmlText camshiftHSVThresholdText = xml_DetectionParameters.CreateTextNode(camshiftHSVThreshold_data);
            XmlText LMSDistanceThresholdText = xml_DetectionParameters.CreateTextNode(toolStripTextBox_LMSDistanceThreshold.Text);
            XmlText KThresholdText = xml_DetectionParameters.CreateTextNode(toolStripTextBox_KThreshold.Text);
            XmlText TargetIndexThresholdText = xml_DetectionParameters.CreateTextNode(numericUpDown_TargetIndexDel.Text);
            //添加元素
            xml_root.AppendChild(newCamshiftHSVThreshold);
            xml_root.AppendChild(newDetectionThreshold);
            xml_root.AppendChild(newShapeCoeff);
            xml_root.AppendChild(newDistanceThreshold);
            xml_root.AppendChild(newMaskThreshold);
            xml_root.AppendChild(newDilateTimes);
            xml_root.AppendChild(newErodeTimes);
            xml_root.AppendChild(newLMSDistanceThreshold);
            xml_root.AppendChild(newKThreshold);
            xml_root.AppendChild(newTargetIndexThreshold);
            newCamshiftHSVThreshold.AppendChild(camshiftHSVThresholdText);
            newDetectionThreshold.AppendChild(detectionThresholdText);
            newShapeCoeff.AppendChild(shapeCoeffText);
            newDistanceThreshold.AppendChild(diatanceThresholdText);
            newMaskThreshold.AppendChild(maskThresholdText);
            newDilateTimes.AppendChild(dilateTimesText);
            newErodeTimes.AppendChild(erodeTimesText);
            newLMSDistanceThreshold.AppendChild(LMSDistanceThresholdText);
            newKThreshold.AppendChild(KThresholdText);
            newTargetIndexThreshold.AppendChild(TargetIndexThresholdText);

            xml_DetectionParameters.Save(mainwindows_detection.path + "/doc/DetectionParameters.xml");
            MessageBox.Show("成功存储特征检测参数!");
        }

        private void button_TemplateMatchSatrt_Click(object sender, EventArgs e)
        {
            imageBox_template.Image = tpl;
            templatematch_flag = true;

            match_reference = match_frame.MatchTemplate(tpl, Emgu.CV.CvEnum.TM_TYPE.CV_TM_CCOEFF_NORMED);
            match_reference.MinMax(out minvalue, out maxvalue, out minloc, out maxloc);
            draw_window.Location = maxloc[0];
            match_frame.Draw(draw_window, new Bgr(Color.Red), 2);
            match_show = match_frame.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
            imageBox_TemplateMatch.Image = match_show;
            //camshift的初始位置
            selectwindow = draw_window;

            richTextBox_Show.SelectionColor = Color.Black;
            richTextBox_Show.AppendText("模版匹配:\n" + "匹配位置：" + maxloc[0].ToString() + "\n匹配值：" + maxvalue[0].ToString() + "\n");
            richTextBox_Show.ScrollToCaret();

            button_reference.Enabled = true;
            button_Continue.Enabled = true;
        }

        private void button_reference_Click(object sender, EventArgs e)
        {
            match_reference_show = match_reference.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR).ConvertScale<Byte>(127,127);
            imageBox_TemplateMatch.Image = match_reference_show;
        }

        private void button_Continue_Click(object sender, EventArgs e)
        {
            if (capture_start)
            {
                button_CamshiftStart.Enabled = true;
                button_backproject.Enabled = true;
                button_Histogram.Enabled = true;
                button_DetectStart.Enabled = true;
                button_MatchRate.Enabled = true;
                if (videotest)//在视频实验中用于记录继续键的按下
                {
                    continue_flag = true;
                }

                button_ChangeTemplate.Enabled = false;
                toolStripButton_Calibration.Enabled = false;

                if (radioButton_RecordAuto.Checked)//自动录制
                {
                    RecordInitial();
                    button_StartRecord.Enabled = false;
                    button_StopRecord.Enabled = false;
                    button_PauseRecord.Enabled = true;
                    checkBox_VideoOriginal.Enabled = false;
                    checkBox_VideoCamshift.Enabled = false;
                    checkBox_VideoDetection.Enabled = false;
                }
                else
                {
                    if (checkBox_VideoOriginal.Checked || checkBox_VideoDetection.Checked || checkBox_VideoCamshift.Checked)
                        button_StartRecord.Enabled = true;
                }
                radioButton_RecordAuto.Enabled = false;
                radioButton_RecordManual.Enabled = false;
                record_Detection = new Image<Bgr, Byte>(frame.Width + tpl.Width, frame.Height);  //初始化特征检测录像
                canrecord = true;
            }
            else
            {
                richTextBox_Show.SelectionColor = Color.Red;
                richTextBox_Show.AppendText("警告：摄像头未打开,无法继续！\n");
                richTextBox_Show.ScrollToCaret();
            }
        }

        private void button_ChangeTemplate_Click(object sender, EventArgs e)
        {
            if (openfile_template.ShowDialog() == DialogResult.OK)
            {
                tpl = new Image<Bgr, Byte>(openfile_template.FileName);
                Init_TemplateRelated();
                imageBox_template.Image = tpl;
                //显示
                richTextBox_Show.SelectionColor = Color.Green;
                richTextBox_Show.AppendText("更换了模版文件：" + video_info.filename + "\n");
                richTextBox_Show.ScrollToCaret();

                canrecord = false;
            }
        }

        private void button_CamshiftStart_Click(object sender, EventArgs e)
        {
            if (button_CamshiftStart.Text == "开始跟踪")
            {
                toolStripButton_ReadCamera.Enabled = false;
                //打开Camshift
                
                if (cal_hist_flag)
                {
                    //计算模版的颜色直方图
                    hsv = frame_camshift.Convert<Hsv, byte>();
                    CvInvoke.cvInRangeS(hsv, new MCvScalar(0, 30, 10, 0), new MCvScalar(180, 256, 256, 0), mask);  //过滤部分信息得到掩码
                    CvInvoke.cvSplit(hsv, hue, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero);
                    CvInvoke.cvSetImageROI(hue, selectwindow);
                    CvInvoke.cvSetImageROI(mask, selectwindow);
                    IntPtr[] img = new IntPtr[1] { hue };
                    CvInvoke.cvCalcHist(img, hist, false, mask);
                    CvInvoke.cvGetMinMaxHistValue(hist, ref hist_minvalue, ref hist_maxvalue, mininx, maxinx);
                    CvInvoke.cvResetImageROI(hue);
                    CvInvoke.cvResetImageROI(mask);
                    trackwin = selectwindow;

                    //绘制颜色直方图
                    CvInvoke.cvZero(histimg);
                    int bin_w = histimg.Width / hist.BinDimension[0].Size;
                    for (int i = 0; i < hist.BinDimension[0].Size; i++)
                    {
                        richTextBox_Show.Invoke(new EventHandler(delegate
                        {
                            richTextBox_Show.SelectionColor = Color.Black;
                            richTextBox_Show.AppendText("[" + i.ToString() + "] " + hist.MatND.ManagedArray.GetValue(i).ToString() + "\n");
                            richTextBox_Show.ScrollToCaret();
                        }));
                        hist.MatND.ManagedArray.SetValue(((Single)hist.MatND.ManagedArray.GetValue(i)) * 255 / hist_maxvalue, i);  //归一化,非常重要！将值映射到0~255
                        int val = (int)(((Single)hist.MatND.ManagedArray.GetValue(i)) * histimg.Height / 255);
                        Rectangle rect = new Rectangle(i * bin_w, histimg.Height - val, bin_w, val);
                        Hsv color_hsv = new Hsv(i * 180 / hist.BinDimension[0].Size, 255, 255);
                        Bgr color_bgr = HSV2BGR(color_hsv);
                        histimg.Draw(rect, color_bgr, -1);
                    }
                }
                
                button_CamshiftStart.Text = "停止跟踪";
                camshift_flag = true;
                thread_camshift = new Thread(CamshiftThread);
                thread_camshift_flag = true;
                thread_camshift.Start();

                if (videotest)  //视频继续播放
                {
                    //video_continue = true;
                    video_continue2 = true;
                }
            }
            else
            {
                //关闭Camshift
                button_CamshiftStart.Text = "开始跟踪";
                camshift_flag = false;
                cal_hist_flag = false; //如果在此处暂停，下次开始时不用重新计算hist
                if (videotest)
                {
                    //video_continue = false;
                    video_continue2 = false;
                }
                if (button_DetectStart.Text == "开始检测")
                {
                    toolStripButton_ReadCamera.Enabled = true;
                }
            }

        }

        private void button_backproject_Click(object sender, EventArgs e)
        {
            if (button_backproject.Text == "反向投影图")
            {
                button_backproject.Text = "返回";
                showstate = CamshiftShow.Backproject;
            }
            else
            {
                button_backproject.Text = "反向投影图";
                showstate = CamshiftShow.Camshift;
            }
        }

        private void button_Histogram_Click(object sender, EventArgs e)
        {
            if (button_Histogram.Text == "颜色直方图")
            {
                button_Histogram.Text = "返回";
                showstate = CamshiftShow.Histogram;
            }
            else
            {
                button_Histogram.Text = "颜色直方图";
                showstate = CamshiftShow.Camshift;
            }
        }

        private void radioButton_SIFT_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_SIFT.Checked == true)
                detect_state = DetectionState.SIFT;
        }

        private void radioButton_CSIFT_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_CSIFT.Checked == true)
                detect_state = DetectionState.CSIFT;
        }

        private void radioButton_SURF_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_SURF.Checked == true)
                detect_state = DetectionState.SURF;
        }

        private void radioButton_USURF_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_USURF.Checked == true)
                detect_state = DetectionState.USURF;
        }

        private void numericUpDown_DetectionThreshold_ValueChanged(object sender, EventArgs e)
        {
            Detection_threshold = (int)Math.Round(numericUpDown_DetectionThreshold.Value);
        }

        private void checkBox_ShowEffectivePoint_CheckedChanged(object sender, EventArgs e)
        {
            if (checkBox_ShowEffectivePoint.Checked)
                show_EffectivePoint = true;
            else
                show_EffectivePoint = false;
        }

        private void numericUpDown_ShapeCoeff_ValueChanged(object sender, EventArgs e)
        {
            Shape_coeff = (float)Math.Round(numericUpDown_ShapeCoeff.Value, 1);
        }

        private void numericUpDown_DistanceThreshold_ValueChanged(object sender, EventArgs e)
        {
            win_diatance_threshold = (int)numericUpDown_DistanceThreshold.Value;
        }

        private void button_DetectStart_Click(object sender, EventArgs e)
        {
            if (button_DetectStart.Text == "开始检测")
            {
                toolStripButton_ReadCamera.Enabled = false;
                //打开检测
                switch (detect_state)
                {
                    case DetectionState.SIFT:
                        {
                            //获得SIFT模版的描述子
                            Image<Gray, byte> tpl_gray = tpl.Convert<Gray, byte>();
                            tpl_draw = tpl.Copy();

                            //检测特征点,计算描述子
                            tplKeyPoints = sift.DetectKeyPointsRaw(tpl_gray, null);
                            tplDescriptors = sift.ComputeDescriptorsRaw(tpl_gray, null, tplKeyPoints);
                            break;
                        }
                    case DetectionState.SURF:
                        {
                            //获得SURF模版的描述子
                            Image<Gray, byte> tpl_gray = tpl.Convert<Gray, byte>();
                            tpl_draw = tpl.Copy();

                            //检测特征点,计算描述子
                            tplKeyPoints = surf.DetectKeyPointsRaw(tpl_gray, null);
                            tplDescriptors = surf.ComputeDescriptorsRaw(tpl_gray, null, tplKeyPoints);
                            break;
                        }
                    case DetectionState.USURF:
                        {
                            //获得U-SURF模版的描述子
                            Image<Gray, byte> tpl_gray = tpl.Convert<Gray, byte>();
                            tpl_draw = tpl.Copy();

                            //检测特征点,计算描述子
                            tplKeyPoints = usurf.DetectKeyPointsRaw(tpl_gray, null);
                            tplDescriptors = usurf.ComputeDescriptorsRaw(tpl_gray, null, tplKeyPoints);
                            break;
                        }
                    case DetectionState.CSIFT:
                        {

                            break;
                        }
                    default:
                        break;
                }
                
                //为匹配器加载模版描述子
                matcher = new BruteForceMatcher<float>(DistanceType.L2);
                matcher.Add(tplDescriptors);

                button_DetectStart.Text = "停止检测";
                radioButton_CSIFT.Enabled = false;
                radioButton_SIFT.Enabled = false;
                radioButton_SURF.Enabled = false;
                radioButton_USURF.Enabled = false;
                detect_flag = true;
                thread_detect = new Thread(DetectionThread);
                thread_detect_flag = true;
                thread_detect.Start();
            }
            else
            {
                //关闭检测
                button_DetectStart.Text = "开始检测";
                detect_flag = false;
                radioButton_CSIFT.Enabled = true;
                radioButton_SIFT.Enabled = true;
                radioButton_SURF.Enabled = true;
                radioButton_USURF.Enabled = true;
                if (button_CamshiftStart.Text == "开始跟踪")
                {
                    toolStripButton_ReadCamera.Enabled = true;
                }
            }
        }

        private void button_MatchRate_Click(object sender, EventArgs e)
        {
            matchrate = true;
        }

        private void button_clear_Click(object sender, EventArgs e)
        {
            richTextBox_Show.Clear();
        }

        private void imageBox_Camshift_MouseDown(object sender, MouseEventArgs e)
        {
            relate_down.X = e.X;
            relate_down.Y = e.Y;
            down_done = true;
        }

        private void imageBox_Camshift_MouseUp(object sender, MouseEventArgs e)
        {
            if (down_done)
            {
                relate_up.X = e.X;
                relate_up.Y = e.Y;

                if ((relate_up.X > relate_down.X) && (relate_up.Y > relate_down.Y))//down左上,up右下
                {
                    mousewindow.X = relate_down.X;
                    mousewindow.Y = relate_down.Y;
                    mousewindow.Width = relate_up.X - relate_down.X;
                    mousewindow.Height = relate_up.Y - relate_down.Y;
                }
                else if ((relate_up.X < relate_down.X) && (relate_up.Y < relate_down.Y))//down右下,up左上
                {
                    mousewindow.X = relate_up.X;
                    mousewindow.Y = relate_up.Y;
                    mousewindow.Width = relate_down.X - relate_up.X;
                    mousewindow.Height = relate_down.Y - relate_up.Y;
                }
                else if ((relate_up.X > relate_down.X) && (relate_up.Y < relate_down.Y))//down左下,up右上
                {
                    mousewindow.X = relate_down.X;
                    mousewindow.Y = relate_up.Y;
                    mousewindow.Width = relate_up.X - relate_down.X;
                    mousewindow.Height = relate_down.Y - relate_up.Y;
                }
                else if ((relate_up.X < relate_down.X) && (relate_up.Y > relate_down.Y))//down右上,up左下
                {
                    mousewindow.X = relate_up.X;
                    mousewindow.Y = relate_down.Y;
                    mousewindow.Width = relate_down.X - relate_up.X;
                    mousewindow.Height = relate_up.Y - relate_down.Y;
                }

                mousewindow.X *= 2;
                mousewindow.Y *= 2;
                mousewindow.Width *= 2;
                mousewindow.Height *= 2;
                mouse_done = true;
                down_done = false;
            }
            flag_firsttime = 5;//防止改变目标后激光点误判
        }

        private void button_IndexDelSure_Click(object sender, EventArgs e)
        {
            index_threshold = (int)numericUpDown_TargetIndexDel.Value;
        }

        private void radioButton_OnlyCamera_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_OnlyCamera.Checked)
                detectoin_tools = DetectionTools.OnlyCamera;
            flag_firsttime = 5;//防止状态切换时距离改变大引起异常
        }

        private void radioButton_OnlyLMS_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_OnlyLMS.Checked)
                detectoin_tools = DetectionTools.OnlyLMS;
            flag_firsttime = 5;//防止状态切换时距离改变大引起异常
        }

        private void radioButton_BothCameraAndLMS_CheckedChanged(object sender, EventArgs e)
        {
            if (radioButton_BothCameraAndLMS.Checked)
                detectoin_tools = DetectionTools.BothCameraAndLMS;
            flag_firsttime = 5; //防止状态切换时距离改变大引起异常
        }

        private void button_StartRecord_Click(object sender, EventArgs e)
        {
            if (checkBox_VideoOriginal.Checked == true)
            {
                try
                {
                    SaveFileDialog saveFileDialog = new SaveFileDialog();
                    saveFileDialog.Title = "保存原始图像";
                    saveFileDialog.Filter = "AVI|*.avi";
                    saveFileDialog.AddExtension = true;
                    if (saveFileDialog.ShowDialog() == DialogResult.OK)
                    {
                        videoRecord_Original = new VideoWriter(saveFileDialog.FileName, //文件名
                            CvInvoke.CV_FOURCC('M', 'J', 'P', 'G'), //编码格式
                            (int)numericUpDown_VideoFPS.Value, //帧率
                            frame.Width, //视频宽度
                            frame.Height, //视频高度
                            true);//彩色
                        recordOriginal_start = true;
                    }
                }
                catch (NullReferenceException excpt)
                {
                    MessageBox.Show(excpt.Message);
                }
            }
            if (checkBox_VideoCamshift.Checked == true)
            {
                try
                {
                    SaveFileDialog saveFileDialog = new SaveFileDialog();
                    saveFileDialog.Title = "保存Camshift跟踪图像";
                    saveFileDialog.Filter = "AVI|*.avi";
                    saveFileDialog.AddExtension = true;
                    if (saveFileDialog.ShowDialog() == DialogResult.OK)
                    {
                        videoRecord_Camshift = new VideoWriter(saveFileDialog.FileName, //文件名
                            CvInvoke.CV_FOURCC('M', 'J', 'P', 'G'), //编码格式
                            (int)numericUpDown_VideoFPS.Value, //帧率
                            camshift_draw.Width, //视频宽度
                            camshift_draw.Height, //视频高度
                            true);//彩色
                        recordCamshift_start = true;
                    }
                }
                catch (NullReferenceException excpt)
                {
                    MessageBox.Show(excpt.Message);
                }
            }
            if (checkBox_VideoDetection.Checked == true)
            {
                try
                {
                    SaveFileDialog saveFileDialog = new SaveFileDialog();
                    saveFileDialog.Title = "保存特征检测图像";
                    saveFileDialog.Filter = "AVI|*.avi";
                    saveFileDialog.AddExtension = true;
                    if (saveFileDialog.ShowDialog() == DialogResult.OK)
                    {
                        videoRecord_Detection = new VideoWriter(saveFileDialog.FileName, //文件名
                            CvInvoke.CV_FOURCC('M', 'J', 'P', 'G'), //编码格式
                            (int)numericUpDown_VideoFPS.Value, //帧率
                            frame.Width + tpl.Width, //视频宽度
                            frame.Height, //视频高度
                            true);//彩色
                        recordDetection_start = true;
                    }
                }
                catch (NullReferenceException excpt)
                {
                    MessageBox.Show(excpt.Message);
                }
            }

            button_StartRecord.Enabled = false;
            button_StopRecord.Enabled = false;
            button_PauseRecord.Enabled = true;
            checkBox_VideoOriginal.Enabled = false;
            checkBox_VideoCamshift.Enabled = false;
            checkBox_VideoDetection.Enabled = false;
        }

        private void button_PauseRecord_Click(object sender, EventArgs e)
        {
            if (button_PauseRecord.Text == "暂停录制")
            {
                button_PauseRecord.Text = "恢复录制";
                record_pause = true;
                button_StopRecord.Enabled = true;
            }
            else
            {
                button_PauseRecord.Text = "暂停录制";
                record_pause = false;
                button_StopRecord.Enabled = false;
            }            
        }

        private void button_StopRecord_Click(object sender, EventArgs e)
        {
            if (checkBox_VideoOriginal.Checked == true)
                videoRecord_Original.Dispose();
            if (checkBox_VideoCamshift.Checked == true)
                videoRecord_Camshift.Dispose();
            if (checkBox_VideoDetection.Checked == true)
                videoRecord_Detection.Dispose();

            record_pause = false;
            recordOriginal_start = false;
            recordCamshift_start = false;
            recordDetection_start = false;

            if (canrecord && (checkBox_VideoCamshift.Checked || checkBox_VideoDetection.Checked || checkBox_VideoOriginal.Checked))
                button_StartRecord.Enabled = true;
            button_PauseRecord.Enabled = false;
            checkBox_VideoOriginal.Enabled = true;
            checkBox_VideoCamshift.Enabled = true;
            checkBox_VideoDetection.Enabled = true;
            radioButton_RecordAuto.Enabled = true;
            radioButton_RecordManual.Enabled = true;
        }

        private void checkBox_VideoOriginal_CheckedChanged(object sender, EventArgs e)
        {
            if (canrecord && (checkBox_VideoOriginal.Checked || checkBox_VideoCamshift.Checked || checkBox_VideoDetection.Checked))
                button_StartRecord.Enabled = true;
            else
                button_StartRecord.Enabled = false;
        }

        private void checkBox_VideoCamshift_CheckedChanged(object sender, EventArgs e)
        {
            if (canrecord && (checkBox_VideoOriginal.Checked || checkBox_VideoCamshift.Checked || checkBox_VideoDetection.Checked))
                button_StartRecord.Enabled = true;
            else
                button_StartRecord.Enabled = false;
        }

        private void checkBox_VideoDetection_CheckedChanged(object sender, EventArgs e)
        {
            if (canrecord && (checkBox_VideoOriginal.Checked || checkBox_VideoCamshift.Checked || checkBox_VideoDetection.Checked))
                button_StartRecord.Enabled = true;
            else
                button_StartRecord.Enabled = false;
        }
        #endregion

        #region 触发中断
        private void capture_ImageGrabbed(object sender, EventArgs arg)
        {
            frame = capture.RetrieveBgrFrame();

            if (cali_flag)
            {
                if (remap_flag)
                {
                    cali.InitUndistortMap(frame.Width, frame.Height, out map1, out map2);
                    remap_flag = false;
                }
                Image<Bgr, Byte> temp = frame.CopyBlank();
                CvInvoke.cvRemap(frame, temp, map1, map2, 0, new MCvScalar(0));
                frame = temp.Copy();
                frame_camshift = frame.Copy();
                detect_target = frame.Copy();
            }
            else
            {
                frame_camshift = frame.Copy();
                detect_target = frame.Copy();
            }

            if (templatematch_flag)
            {
                match_frame = frame.Copy();
                templatematch_flag = false;
            }

            frame_rawshow = frame.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
            imageBox_rawvideo.Image = frame_rawshow;

            record_frame = frame.Copy();
            if (recordOriginal_start && !record_pause) //录制原始视频
            {
                videoRecord_Original.WriteFrame(record_frame);
            }
            if (recordCamshift_start && !record_pause)//录制Camshift视频
            {
                videoRecord_Camshift.WriteFrame(record_Camshift);
            }
            if (recordDetection_start && !record_pause)//录制特征检测视频
            {
                videoRecord_Detection.WriteFrame(record_Detection);
            }

            //显示消耗时间
            end1 = System.DateTime.Now;
            time1 = end1 - start1;
            if (time_flag)
            {
                richTextBox_Show.Invoke(new EventHandler(delegate
                {
                    richTextBox_Show.SelectionColor = Color.Black;
                    richTextBox_Show.AppendText("显示耗时：" + time1.Milliseconds.ToString() + "\nCamshift耗时：" + time2.Milliseconds.ToString() +
                                                "\n特征检测耗时：" + time3.Milliseconds.ToString() + "\nLMS111传输周期："+ time4.Milliseconds.ToString() +
                                                "\n(描述子生成：" + time_detect.Milliseconds.ToString() + "  描述子匹配：" + time_match.Milliseconds.ToString() + ")\n");
                    richTextBox_Show.ScrollToCaret();
                }));
            }
            start1 = end1;
        }

        private void CamshiftThread()
        {
            while (camshift_flag)
            {
                IntPtr[] img;
                hsv = frame_camshift.Convert<Hsv, byte>();
                CvInvoke.cvInRangeS(hsv, new MCvScalar(0, 30, 10, 0), new MCvScalar(180, 256, 256, 0), mask);  //过滤部分信息得到掩码
                CvInvoke.cvSplit(hsv, hue, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero);
                #region 重新选择初始化窗口
                if (mouse_done) //重新选择了初始窗口
                {
                    CvInvoke.cvSetImageROI(hue, mousewindow);
                    CvInvoke.cvSetImageROI(mask, mousewindow);
                    img = new IntPtr[1] { hue };
                    CvInvoke.cvCalcHist(img, hist, false, mask);
                    CvInvoke.cvGetMinMaxHistValue(hist, ref hist_minvalue, ref hist_maxvalue, mininx, maxinx);
                    CvInvoke.cvResetImageROI(hue);
                    CvInvoke.cvResetImageROI(mask);
                    trackwin = mousewindow;

                    //绘制颜色直方图
                    CvInvoke.cvZero(histimg);
                    int bin_w = histimg.Width / hist.BinDimension[0].Size;
                    for (int i = 0; i < hist.BinDimension[0].Size; i++)
                    {
                        richTextBox_Show.Invoke(new EventHandler(delegate
                        {
                            richTextBox_Show.SelectionColor = Color.Black;
                            richTextBox_Show.AppendText("[" + i.ToString() + "] " + hist.MatND.ManagedArray.GetValue(i).ToString() + "\n");
                            richTextBox_Show.ScrollToCaret();
                        }));
                        hist.MatND.ManagedArray.SetValue(((Single)hist.MatND.ManagedArray.GetValue(i)) * 255 / hist_maxvalue, i);  //归一化,非常重要！将值映射到0~255
                        int val = (int)(((Single)hist.MatND.ManagedArray.GetValue(i)) * histimg.Height / 255);
                        Rectangle rect = new Rectangle(i * bin_w, histimg.Height - val, bin_w, val);
                        Hsv color_hsv = new Hsv(i * 180 / hist.BinDimension[0].Size, 255, 255);
                        Bgr color_bgr = HSV2BGR(color_hsv);
                        histimg.Draw(rect, color_bgr, -1);
                    }
                    mouse_done = false;
                }
                #endregion

                #region 计算和对比目标直方图
                CvInvoke.cvInRangeS(hsv, new MCvScalar(Hmin_target, Smin_target, Vmin_target, 0), new MCvScalar(Hmax_target, Smax_target, Vmax_target, 0), mask_hist);  //过滤部分信息得到掩码
                CvInvoke.cvSetImageROI(hue, trackwin);
                CvInvoke.cvSetImageROI(mask_hist, trackwin);
                img = new IntPtr[1] { hue };
                CvInvoke.cvCalcHist(img, hist_target, false, mask_hist);
                CvInvoke.cvResetImageROI(hue);
                CvInvoke.cvResetImageROI(mask_hist);
                CvInvoke.cvGetMinMaxHistValue(hist_target, ref hist_target_minvalue, ref hist_target_maxvalue, mininx_target, maxinx_target);
                for (int i = 0; i < hist_target.BinDimension[0].Size; i++)
                {
                    hist_target.MatND.ManagedArray.SetValue(((Single)hist_target.MatND.ManagedArray.GetValue(i)) * 255 / hist_target_maxvalue, i);  //归一化,非常重要！将值映射到0~255
                }
                //计算直方图相似度
                if (flag_EMD)  //计算陆地移动距离EMD
                {
                    Matrix<Single> sig1,sig2;
                    sig1 = new Matrix<Single>(hist.BinDimension[0].Size, 2);
                    sig2 = new Matrix<Single>(hist_target.BinDimension[0].Size, 2);
                    for (int i = 0; i < hist.BinDimension[0].Size; i++)
                    {
                        sig1[i, 0] = (Single)hist.MatND.ManagedArray.GetValue(i);
                        sig1[i, 1] = i;
                    }
                    for (int i = 0; i < hist_target.BinDimension[0].Size; i++)
                    {
                        sig2[i, 0] = (Single)hist_target.MatND.ManagedArray.GetValue(i);
                        sig2[i, 1] = i;
                    }
                    coeff = CvInvoke.cvCalcEMD2(sig1, sig2, DIST_TYPE.CV_DIST_L1, null, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero, IntPtr.Zero);
                }
                else //对比两个直方图
                    coeff = CvInvoke.cvCompareHist(hist, hist_target, comp_method);

                //绘制目标图像的颜色直方图
                if (ref_open)
                { 
                    //绘制颜色直方图
                    if (hist_target_show)
                    {
                        CvInvoke.cvZero(histimg_target);
                        int bin_w_target = histimg_target.Width / hist_target.BinDimension[0].Size;
                        for (int i = 0; i < hist_target.BinDimension[0].Size; i++)
                        {
                            if (histdata_target_show && data_show)  //判断显示直方图数据
                            {
                                detection_ref.richTextBox_Show.Invoke(new EventHandler(delegate
                                {
                                    detection_ref.richTextBox_Show.SelectionColor = Color.Black;
                                    detection_ref.richTextBox_Show.AppendText("[" + i.ToString() + "] " + hist_target.MatND.ManagedArray.GetValue(i).ToString() + "\n");
                                    detection_ref.richTextBox_Show.ScrollToCaret();
                                })); //此步骤很费时间,大概会耗时150ms左右
                            }
                            int val_target = (int)(((Single)hist_target.MatND.ManagedArray.GetValue(i)) * histimg_target.Height / 255);
                            Rectangle rect_target = new Rectangle(i * bin_w_target, histimg_target.Height - val_target, bin_w_target, val_target);
                            Hsv color_hsv_target = new Hsv(i * 180 / hist_target.BinDimension[0].Size, 255, 255);
                            Bgr color_bgr_target = HSV2BGR(color_hsv_target);
                            histimg_target.Draw(rect_target, color_bgr_target, -1);
                        }
                    }
                    histimg_target_show = histimg_target.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
                    detection_ref.imageBox_Ref1.Image = histimg_target_show;

                    //显示相似度数据
                    if (coeffdata_show && data_show) //判断显示直方图相似度
                    {
                        detection_ref.richTextBox_Show.Invoke(new EventHandler(delegate
                        {
                            detection_ref.richTextBox_Show.SelectionColor = Color.Black;
                            detection_ref.richTextBox_Show.AppendText("直方图相似度：" + coeff.ToString() + "\n");
                            detection_ref.richTextBox_Show.ScrollToCaret();
                        }));
                    }
                }
                #endregion

                #region 判断目标是否丢失并做出决策
                //判断目标是否丢失
                LoseState lose_state = TargetLose();
                switch (lose_state)
                {
                    case LoseState.BothOK:  //都检测到目标
                        {
                            distance_center = CalculateDistance(camshift_center, detection_center); //计算中心偏差

                            if (distance_center > win_diatance_threshold)
                            {
                                trackwin = trackwin_temp;
                            }

                            if (checkBox＿ShowDistanceThreshold.Checked)
                            {
                                richTextBox_Show.Invoke(new EventHandler(delegate
                                {
                                    richTextBox_Show.SelectionColor = Color.Black;
                                    richTextBox_Show.AppendText("Camshift和特征检测中心偏差：" + distance_center.ToString() + "\n");
                                    richTextBox_Show.ScrollToCaret();
                                }));
                            }
                            target_dataupdate_Camera = true;
                            break;
                        }
                    case LoseState.CamshiftLose:  //Camshift丢失目标
                        {
                            trackwin = trackwin_temp;

                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Purple;
                                richTextBox_Show.AppendText("(双开)Camshift目标丢失！\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                            target_dataupdate_Camera = true;
                            break;
                        }
                    case LoseState.DetectionLose: //Detection丢失目标
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Purple;
                                richTextBox_Show.AppendText("(双开)特征检测目标丢失！\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                            target_dataupdate_Camera = true;
                            break;
                        }
                    case LoseState.BothLose:  //都失去目标
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Red;
                                richTextBox_Show.AppendText("Camsift和特征检测目标均丢失！\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                            target_dataupdate_Camera = false;
                            break;
                        }
                    case LoseState.OnlyCamshift_Lose://只有Camshift的时候失去目标
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Red;
                                richTextBox_Show.AppendText("Camsift目标丢失！\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                            target_dataupdate_Camera = false;
                            break;
                        }
                    case LoseState.OnlyCamshift_OK: //只有Camshift的时候检测到目标
                        {

                            target_dataupdate_Camera = true;
                            break;
                        }
                    default:
                        break;
                }
                #endregion

                #region Camshift计算
                img = new IntPtr[1]{hue};
                CvInvoke.cvCalcBackProject(img, backproject, hist);
                CvInvoke.cvAnd(backproject, mask, backproject, IntPtr.Zero);
                CvInvoke.cvCamShift(backproject, trackwin, new MCvTermCriteria(10, 0.5), out trackcomp, out trackbox);
                trackwin = trackcomp.rect;

                //计算中心点
                camshift_center = CalculateCross(new PointF[4]{ 
                                                    new PointF(trackwin.Left, trackwin.Bottom),
                                                    new PointF(trackwin.Right, trackwin.Bottom),
                                                    new PointF(trackwin.Right, trackwin.Top),
                                                    new PointF(trackwin.Left, trackwin.Top)});

                start7 = System.DateTime.Now;
                if (!mask_cancel)
                {
                    //根据backproject图像得到特征检测的掩码
                    CvInvoke.cvInRangeS(backproject, new MCvScalar(mask_threshold, 0, 0, 0), new MCvScalar(255, 0, 0, 0), detectionMask);
                    if ((erode_times > 0) && (!erode_cancel))//若erode_times为0,则表示不进行腐蚀操作
                        CvInvoke.cvErode(detectionMask, detectionMask, IntPtr.Zero, erode_times);
                    if ((dilate_times > 0) && (!dilate_cancel))//若dilate_times为0,则表示不进行膨胀操作
                        CvInvoke.cvDilate(detectionMask, detectionMask, IntPtr.Zero, dilate_times);//膨胀操作,将颜色相似的区域连起来,10以内耗时在1ms以内
                }
                else
                {
                    detectionMask.SetValue(255);
                }
                end7 = System.DateTime.Now;
                time_test = end7 - start7;

                if (ref_open && mask_show)
                    detection_ref.imageBox_Ref2.Image = detectionMask.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);

                #endregion

                #region LMS111数据更新
                if (!videotest)
                {
                    if (LMS111_serial.IsOpen)
                    {
                        //更新数据
                        if (LMS111_done)
                        {
                            #region 图像信息目标识别
                            if (target_dataupdate_Camera) //清零
                            {
                                target_CameraX_min = 640;
                                target_CameraX_max = 0;

                                for (int i = 0; i < LMS_NUM; i++)
                                {
                                    LMS_u_temp[i] = LMS_u[i];
                                    LMS_v_temp[i] = LMS_v[i];

                                    //找出目标中的激光点
                                    if ((LMS_u_temp[i] >= trackwin.X) && (LMS_u_temp[i] <= (trackwin.X + trackwin.Width))
                                        && (LMS_v_temp[i] >= trackwin.Y) && (LMS_v_temp[i] <= (trackwin.Y + trackwin.Height)))
                                    {
                                        if (LMS_u_temp[i] <= target_CameraX_min)
                                        {
                                            target_CameraX_min = LMS_u_temp[i];
                                            target_Cameraindex_max = i;  //注意此处的交换,图像中的顺序和激光序列相反
                                        }
                                        if (LMS_u_temp[i] >= target_CameraX_max)
                                        {
                                            target_CameraX_max = LMS_u_temp[i];
                                            target_Cameraindex_min = i; //注意此处的交换,图像中的顺序和激光序列相反
                                        }
                                        LMSdataValid = true;
                                    }
                                }
                                //以中点为目标点,计算相关参数
                                if (LMSdataValid)
                                {
                                    target_Cameraindex = (target_Cameraindex_min + target_Cameraindex_max) / 2;
                                    angle_Cameratemp = target_Cameraindex * 0.5 - 60;
                                    target_X_Cameratemp = LMS_Z[target_Cameraindex];  //注意此时的坐标系，和标定的时候的坐标系不同
                                    target_Y_Cameratemp = -LMS_X[target_Cameraindex];  //注意此时的坐标系，和标定的时候的坐标系不同
                                    distance_Cameratemp = Math.Sqrt(target_X_Cameratemp * target_X_Cameratemp + target_Y_Cameratemp * target_Y_Cameratemp);
                                }
                            }
                            #endregion

                            #region 激光信息目标识别
                            target_LMSindex_max = target_LMSindex;
                            target_LMSindex_min = target_LMSindex;

                            if (K_threshold > 0) //切换识别模式
                            {
                                //采用三个激光点计算三角形顶角的方式
                                //while ((target_LMSindex_max - 1) >= 0 && (target_LMSindex_max + 1) < LMS_NUM && 
                                //    KDelCalcu(LMS_data_temp[target_LMSindex_max - 1], LMS_data_temp[target_LMSindex_max], LMS_data_temp[target_LMSindex_max + 1], 0.5 * Math.PI / 180) > K_threshold)
                                //{
                                //    target_LMSindex_max++;
                                //}
                                //while ((target_LMSindex_min - 1) >= 0 && (target_LMSindex_min + 1) < LMS_NUM && 
                                //    KDelCalcu(LMS_data_temp[target_LMSindex_min - 1], LMS_data_temp[target_LMSindex_min], LMS_data_temp[target_LMSindex_min + 1], 0.5 * Math.PI / 180) > K_threshold)
                                //{
                                //    target_LMSindex_min--;
                                //}

                                //采用两个激光点判断距离的方式
                                while ((target_LMSindex_max - 1) >= 0 && (target_LMSindex_max + 1) < LMS_NUM &&
                                    !KJudgeIsEdge(LMS_data_temp[target_LMSindex_max], LMS_data_temp[target_LMSindex_max+1], 0.5 * Math.PI / 180, 0, K_threshold * Math.PI / 180))
                                {
                                    target_LMSindex_max++;
                                }
                                while ((target_LMSindex_min - 1) >= 0 && (target_LMSindex_min + 1) < LMS_NUM &&
                                    !KJudgeIsEdge(LMS_data_temp[target_LMSindex_min], LMS_data_temp[target_LMSindex_min-1], 0.5 * Math.PI / 180, 0, K_threshold * Math.PI / 180))
                                {
                                    target_LMSindex_min--;
                                }
                            }
                            else
                            {
                                while (target_LMSindex_min >= 0 && LMS_data_temp[target_LMSindex_min] < LMS_data_temp[target_LMSindex] * 1.1
                                && LMS_data_temp[target_LMSindex_min] > LMS_data_temp[target_LMSindex] * 0.9)
                                {
                                    target_LMSindex_min--;
                                }
                                target_LMSindex_min++;
                                while (target_LMSindex_max < LMS_NUM && LMS_data_temp[target_LMSindex_max] < LMS_data_temp[target_LMSindex] * 1.1
                                    && LMS_data_temp[target_LMSindex_max] > LMS_data_temp[target_LMSindex] * 0.9)
                                {
                                    target_LMSindex_max++;
                                }
                                target_LMSindex_max--;
                            }

                            if (target_LMSindex_max < target_LMSindex_min)  //判断激光识别数据是否可靠
                            {
                                richTextBox_Show.BeginInvoke(new EventHandler(delegate
                                {
                                    richTextBox_Show.SelectionColor = Color.Red;
                                    richTextBox_Show.AppendText("激光数据目标检测丢失目标！\n");
                                    richTextBox_Show.ScrollToCaret();
                                }));
                            }

                            target_LMSindex = (target_LMSindex_max + target_LMSindex_min)/2;
                            angle_LMStemp = target_LMSindex * 0.5 - 60;
                            target_X_LMStemp = LMS_Z[target_LMSindex];  //注意此时的坐标系，和标定的时候的坐标系不同
                            target_Y_LMStemp = -LMS_X[target_LMSindex];  //注意此时的坐标系，和标定的时候的坐标系不同
                            distance_LMStemp = Math.Sqrt(target_X_LMStemp * target_X_LMStemp + target_Y_LMStemp * target_Y_LMStemp);
                            #endregion

                            int index_Del = Math.Abs(target_Cameraindex - target_LMSindex); //两个工具目标检测偏差
                            this.BeginInvoke(new EventHandler(delegate
                            {
                                toolStripStatusLabel_IndexDel.Text = index_Del.ToString();
                            }));

                            #region 检测工具选择
                            switch (detectoin_tools)
                            {
                                case DetectionTools.OnlyCamera:
                                    {
                                        if (LMSdataValid)
                                        {
                                            target_index_max = target_Cameraindex_max;
                                            target_index_min = target_Cameraindex_min;
                                            target_index = target_Cameraindex;
                                            angle_temp = angle_Cameratemp;
                                            target_X_temp = target_X_Cameratemp;
                                            target_Y_temp = target_Y_Cameratemp;
                                            distance_temp = distance_Cameratemp;
                                            LMSdataValid = false;

                                            if (index_Del > index_threshold)
                                                target_LMSindex = target_Cameraindex; //作为激光目标识别的初始位置
                                        }
                                        isLMSUsed = false;
                                        this.BeginInvoke(new EventHandler(delegate
                                        {
                                            toolStripStatusLabel_DetectionStatus.Text = "采用图像数据";
                                        }));
                                        break;
                                    }
                                case DetectionTools.OnlyLMS:
                                    {
                                        target_index_max = target_LMSindex_max;
                                        target_index_min = target_LMSindex_min;
                                        target_index = target_LMSindex;
                                        angle_temp = angle_LMStemp;
                                        target_X_temp = target_X_LMStemp;
                                        target_Y_temp = target_Y_LMStemp;
                                        distance_temp = distance_LMStemp;
                                        isLMSUsed = true;
                                        this.BeginInvoke(new EventHandler(delegate
                                        {
                                            toolStripStatusLabel_DetectionStatus.Text = "采用激光数据";
                                        })); 
                                        break;
                                    }
                                case DetectionTools.BothCameraAndLMS:
                                    {
                                        if (target_dataupdate_Camera)
                                        {
                                            if (LMSdataValid)
                                            {
                                                target_index_max = target_Cameraindex_max;
                                                target_index_min = target_Cameraindex_min;
                                                target_index = target_Cameraindex;
                                                angle_temp = angle_Cameratemp;
                                                target_X_temp = target_X_Cameratemp;
                                                target_Y_temp = target_Y_Cameratemp;
                                                distance_temp = distance_Cameratemp;
                                                LMSdataValid = false;

                                                if (index_Del > index_threshold)
                                                    target_LMSindex = target_Cameraindex; //作为激光目标识别的初始位置
                                            }
                                            isLMSUsed = false;
                                            this.BeginInvoke(new EventHandler(delegate
                                            {
                                                toolStripStatusLabel_DetectionStatus.Text = "采用图像数据";
                                            }));
                                        }
                                        else
                                        {
                                            target_index_max = target_LMSindex_max;
                                            target_index_min = target_LMSindex_min;
                                            target_index = target_LMSindex;
                                            angle_temp = angle_LMStemp;
                                            target_X_temp = target_X_LMStemp;
                                            target_Y_temp = target_Y_LMStemp;
                                            distance_temp = distance_LMStemp;
                                            isLMSUsed = true;
                                            this.BeginInvoke(new EventHandler(delegate
                                            {
                                                toolStripStatusLabel_DetectionStatus.Text = "采用激光数据";
                                            }));
                                        }
                                        break;
                                    }
                                default:
                                    break;
                            }
                            #endregion

                            if (flag_firsttime > 0) //避免第一次误判
                            {
                                distance = distance_temp;
                                flag_firsttime--;
                            }

                            if (control_open) //显示控制界面的激光数据
                            {
                                vehicle_control.chart_LMS111dataShow.BeginInvoke(new EventHandler(delegate
                                {
                                    LMSdataProcessingInControl(target_index_min, target_index_max, isLMSUsed);//在激光示意图中显示相应的激光点
                                }));
                            }

                            if (LMS_distance_threshold <= 0 || Math.Abs(distance - distance_temp) < LMS_distance_threshold) //判断目标距离是否合理
                            {
                                angle = angle_temp;
                                target_X = target_X_temp;
                                target_Y = target_Y_temp;
                                distance = distance_temp;
                            }
                            else
                            {
                                richTextBox_Show.BeginInvoke(new EventHandler(delegate
                                {
                                    richTextBox_Show.SelectionColor = Color.Red;
                                    richTextBox_Show.AppendText("目标距离不合理！\n");
                                    richTextBox_Show.ScrollToCaret();
                                }));
                            }

                            this.BeginInvoke(new EventHandler(delegate
                            {
                                toolStripStatusLabel_Distance.Text = distance.ToString("0.0") + "mm";
                                toolStripStatusLabel_Angle.Text = angle.ToString("0.0") + "°";
                                toolStripStatusLabel_TargetX.Text = target_X.ToString("0.0") + "mm";
                                toolStripStatusLabel_TargetY.Text = target_Y.ToString("0.0") + "mm";
                            }));

                            LMS111_done = false;
                            LMS111_serial.DiscardInBuffer();
                            LMS111_serial.Write(cmd_single, 0, cmd_single.Length);
                        }
                    }
                }
                #endregion

                #region 选择显示的图像
                switch (showstate)
                {
                    case CamshiftShow.Camshift:
                        {
                            camshift_draw = frame_camshift.Convert<Bgr,byte>();  //这一步可以消除搜索框的闪烁

                            if (!videotest)
                            {
                                if (LMS111_serial.IsOpen)
                                {
                                    //画点
                                    for (int i = 0; i < 241; i++)
                                    {
                                        if (LMS_u_temp[i] < 640 && LMS_u_temp[i] > 0 && LMS_v_temp[i] < 480 && LMS_v_temp[i] > 0)
                                        {
                                            camshift_draw.Draw(new CircleF(new Point(LMS_u_temp[i], LMS_v_temp[i]), 3), new Bgr(Color.Blue), 3);
                                        }
                                        camshift_draw.Draw(new CircleF(new Point(LMS_u_temp[target_Cameraindex_min], LMS_v_temp[target_Cameraindex_min]), 3), new Bgr(Color.Yellow), 3);
                                        camshift_draw.Draw(new CircleF(new Point(LMS_u_temp[target_Cameraindex_max], LMS_v_temp[target_Cameraindex_max]), 3), new Bgr(Color.Yellow), 3);
                                        camshift_draw.Draw(new CircleF(new Point(LMS_u_temp[target_Cameraindex], LMS_v_temp[target_Cameraindex]), 4), new Bgr(Color.Green), 4);
                                    }
                                }
                            }
                            
                            camshift_draw.Draw(trackwin, new Bgr(Color.Red), 2);
                            //camshift_draw.Draw(mousewindow, new Bgr(Color.Blue), 2); //test
                            camshift_draw.Draw(new CircleF(camshift_center, 3), new Bgr(Color.Yellow), 3);//绘制camshift跟踪框的中点
                            camshift_show = camshift_draw.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
                            imageBox_Camshift.Image = camshift_show;
                            if (zoom_open)   //子窗口关闭后就不再调用
                            {
                                zoom.imageBox_CamshiftZoom.Image = camshift_draw;
                            }

                            record_Camshift = camshift_draw.Copy();
                            break;
                        }
                    case CamshiftShow.Backproject:
                        {
                            backproject_draw = backproject.Convert<Bgr, byte>();
                            backproject_draw.Draw(trackwin, new Bgr(Color.Red), 2);
                            camshift_draw.Draw(new CircleF(camshift_center, 3), new Bgr(Color.Yellow), 3);//绘制camshift跟踪框的中点
                            camshift_show = backproject_draw.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
                            imageBox_Camshift.Image = camshift_show;
                            if (zoom_open)
                            {
                                zoom.imageBox_CamshiftZoom.Image = camshift_draw;
                            }

                            record_Camshift = backproject_draw.Copy();
                            break;
                        }
                    case CamshiftShow.Histogram:
                        {
                            camshift_show = histimg.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
                            imageBox_Camshift.Image = camshift_show;
                            if (zoom_open)
                            {
                                zoom.imageBox_CamshiftZoom.Image = camshift_draw;
                            }
                            record_Camshift = histimg.Copy();
                            break;
                        }
                    default:
                        break;
                }
                #endregion

                end2 = System.DateTime.Now;
                time2 = end2 - start2;
                start2 = end2;
            }
        }

        private void DetectionThread()
        {
            while (detect_flag)
            {
                start5 = System.DateTime.Now;  //描述子生成计时（测试）
                switch (detect_state)
                {
                    case DetectionState.SIFT:
                        {
                            Image<Gray, byte> target_gray = detect_target.Convert<Gray, byte>();
                            target_draw = detect_target.Copy();

                            //检测特征点,计算描述子
                            targetKeyPoints = sift.DetectKeyPointsRaw(target_gray, detectionMask);
                            targetDescriptors = sift.ComputeDescriptorsRaw(target_gray, detectionMask, targetKeyPoints);
                            break;
                        }
                    case DetectionState.SURF:
                        {
                            Image<Gray, byte> target_gray = detect_target.Convert<Gray, byte>();
                            target_draw = detect_target.Copy();

                            //检测特征点,计算描述子
                            targetKeyPoints = surf.DetectKeyPointsRaw(target_gray, detectionMask);
                            targetDescriptors = surf.ComputeDescriptorsRaw(target_gray, detectionMask, targetKeyPoints);
                            break;
                        }
                    case DetectionState.USURF:
                        {
                            Image<Gray, byte> target_gray = detect_target.Convert<Gray, byte>();
                            target_draw = detect_target.Copy();

                            //检测特征点,计算描述子
                            targetKeyPoints = usurf.DetectKeyPointsRaw(target_gray, detectionMask);
                            targetDescriptors = usurf.ComputeDescriptorsRaw(target_gray, detectionMask, targetKeyPoints);
                            break;
                        }
                    case DetectionState.CSIFT:
                        {

                            break; 
                        }
                    default:
                        break;
                }
                end5 = System.DateTime.Now;   //描述子生成计时（测试）
                time_detect = end5 - start5;  //描述子生成计时（测试）
                
                //匹配特征点
                start6 = System.DateTime.Now;   //匹配计时（测试）
                int inliers = 0;
                if (targetDescriptors != null)  //判断可否生成描述子
                {
                    indices = new Matrix<int>(targetDescriptors.Rows, k);
                    dist = new Matrix<float>(targetDescriptors.Rows, k);
                    matcher.KnnMatch(targetDescriptors, indices, dist, k, null); //匹配
                    detect_mask = new Matrix<byte>(dist.Rows, 1);
                    detect_mask.SetValue(255);
                    Features2DToolbox.VoteForUniqueness(dist, uniquenessThreshold, detect_mask); //获取掩码
                    inliers = CvInvoke.cvCountNonZero(detect_mask); //统计匹配点个数
                }
                else
                {
                    inliers = 0; //此时没有描述子,因此也将绘制匹配图的掩码全置0
                    detect_mask.SetZero();
                }

                if (inliers >= 4)
                {
                    inliers = Features2DToolbox.VoteForSizeAndOrientation(tplKeyPoints, targetKeyPoints, indices, detect_mask, 1.5, 20);
                    if (inliers >= 4)
                    {
                        homography = Features2DToolbox.GetHomographyMatrixFromMatchedFeatures(tplKeyPoints, targetKeyPoints, indices, detect_mask, 5);
                        //显示匹配率
                        if (matchrate)
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Black;
                                richTextBox_Show.AppendText("匹配点个数："+indices.Rows.ToString()+ "\n有效匹配点个数："+ inliers.ToString()+"\n匹配率："+ (((float)inliers)/((float)indices.Rows)).ToString() +"\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                            matchrate = false;
                        }
                        if (show_EffectivePoint)
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Black;
                                richTextBox_Show.AppendText("有效匹配点个数：" + inliers.ToString() + "\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                        }
                        
                        if (inliers >= Detection_threshold)//有效点大于阈值则找到目标
                            detection_lose1 = false;
                        else
                            detection_lose1 = true;
                    }
                    else
                    {
                        if (!camshift_flag) //当只开特征检测的时候显示丢失信息
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Red;
                                richTextBox_Show.AppendText("特征检测目标丢失！(匹配点少于4个)\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                        }
                        detection_lose1 = true;
                    }
                }
                else
                {
                    if (!camshift_flag) //当只开特征检测的时候显示丢失信息
                    {
                        richTextBox_Show.Invoke(new EventHandler(delegate
                        {
                            richTextBox_Show.SelectionColor = Color.Red;
                            richTextBox_Show.AppendText("特征检测目标丢失！(匹配点少于4个)\n");
                            richTextBox_Show.ScrollToCaret();
                        }));
                    }
                    detection_lose1 = true;
                }

                //合成输出图像
                detect_draw = Features2DToolbox.DrawMatches(tpl_draw, tplKeyPoints, target_draw, targetKeyPoints, indices, 
                                                            new Bgr(255, 0, 0), new Bgr(0, 0, 255), detect_mask, Features2DToolbox.KeypointDrawType.DEFAULT); //耗时2~3ms

                if (homography != null)
                {
                    //绘制矩形框
                    Rectangle rect = tpl_draw.ROI;
                    PointF[] pts = new PointF[] { 
                                                    new PointF(rect.Left, rect.Bottom),
                                                    new PointF(rect.Right, rect.Bottom),
                                                    new PointF(rect.Right, rect.Top),
                                                    new PointF(rect.Left, rect.Top)};
                    homography.ProjectPoints(pts);//将模版的四个角点投影到图像中
                    if (RectJudge(pts))//判断四个角点是否合理
                    {
                        detection_lose2 = false;
                        detect_draw.DrawPolyline(Array.ConvertAll<PointF, Point>(pts, Point.Round), true, new Bgr(Color.Red), 5);
                        //画中心点
                        detection_center = CalculateCross(pts);
                        detect_draw.Draw(new CircleF(detection_center, 3), new Bgr(Color.Yellow), 3);
                        //计算待选框
                        trackwin_temp = GenerateCamshifWin(pts);
                    }
                    else
                    {
                        detection_lose2 = true;
                    }
                    
                    homography = null;
                }

                //联合判断特征检测是否成功
                if (detection_lose2)
                {
                    detection_lose = true;
                }
                else
                {
                    detection_lose = detection_lose1;
                }

                detect_show = detect_draw.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
                imageBox_Detection.Image = detect_show;
                if (zoom_open)
                {
                    zoom.imageBox_DetectionZoom.Image = detect_draw;
                }

                end6 = System.DateTime.Now;  //匹配计时（测试）
                time_match = end6 - start6;  //匹配计时（测试）

                end3 = System.DateTime.Now;  //一个检测周期计时（测试）
                time3 = end3 - start3;
                start3 = end3;

                //录像
                record_Detection = detect_draw.Copy();
            }
            matcher.Dispose();
        }

        private void VideoThread()
        {
            start1 = System.DateTime.Now;
            while(video_continue)
            {
                if ((frequency > 0) && (continue_flag) && (video_continue2))
                {
                    for (int i = 0; i < frequency; i++)
                    {
                        frame = capture.QueryFrame();
                        frame_temp = frame;
                        frame_count++;
                    }
                    frequency = 0; //置零

                    if (frame_count > (video_info.frameCount - 1))
                    {
                        richTextBox_Show.Invoke(new EventHandler(delegate
                        {
                            richTextBox_Show.SelectionColor = Color.Red;
                            richTextBox_Show.AppendText("视频已到末尾！\n");
                            richTextBox_Show.ScrollToCaret();
                        }));
                        video_continue = false;
                    }
                }
                else
                {
                    frame = frame_temp;
                }

                if (frame == null) //调试的过程中发现有时候frame会为null
                {
                    richTextBox_Show.Invoke(new EventHandler(delegate
                    {
                        richTextBox_Show.SelectionColor = Color.Red;
                        richTextBox_Show.AppendText("视频帧丢失！\n");
                        richTextBox_Show.ScrollToCaret();
                    }));
                }
                else
                {
                    if (cali_flag)
                    {
                        if (remap_flag)
                        {
                            cali.InitUndistortMap(frame.Width, frame.Height, out map1, out map2);
                            remap_flag = false;
                        }
                        Image<Bgr, Byte> temp = frame.CopyBlank();
                        CvInvoke.cvRemap(frame, temp, map1, map2, 0, new MCvScalar(0));
                        frame = temp.Copy();
                        frame_camshift = frame.Copy();
                        detect_target = frame.Copy();
                    }
                    else  //在采用视频数据实验的时候可以不需要校正,因为有些视频可能是已经校正过的
                    {
                        frame_camshift = frame.Copy();
                        detect_target = frame.Copy();
                    }

                    if (templatematch_flag)
                    {
                        match_frame = frame.Copy();
                        templatematch_flag = false;
                    }

                    frame_rawshow = frame.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
                    imageBox_rawvideo.Image = frame_rawshow;

                    record_frame = frame.Copy();
                    if (recordOriginal_start && !record_pause) //录制原始视频
                    {
                        videoRecord_Original.WriteFrame(record_frame);
                    }
                    if (recordCamshift_start && !record_pause)//录制Camshift视频
                    {
                        videoRecord_Camshift.WriteFrame(record_Camshift);
                    }
                    if (recordDetection_start && !record_pause)//录制特征检测视频
                    {
                        videoRecord_Detection.WriteFrame(record_Detection);
                    }

                    //显示消耗时间
                    end1 = System.DateTime.Now;
                    time1 = end1 - start1;
                    if (time_flag)
                    {
                        richTextBox_Show.Invoke(new EventHandler(delegate
                        {
                            richTextBox_Show.SelectionColor = Color.Black;
                            richTextBox_Show.AppendText("显示耗时：" + time1.Milliseconds.ToString() + "\nCamshift耗时：" + time2.Milliseconds.ToString() +
                                                        "\n特征检测耗时：" + time3.Milliseconds.ToString() + "\nLMS111传输周期：" + time4.Milliseconds.ToString() +
                                                        "\n(描述子生成：" + time_detect.Milliseconds.ToString() + "  描述子匹配：" + time_match.Milliseconds.ToString() + ")\n");
                            richTextBox_Show.ScrollToCaret();
                        }));
                    }
                    start1 = end1;

                    //根据显示消耗时间计算下一次的freaquecy
                    double f = 1000.0 / video_info.fps;
                    if (time1_sum < f)
                        time1_sum += time1.Milliseconds;
                    else
                    {
                        if (time_flag)
                        {
                            richTextBox_Show.Invoke(new EventHandler(delegate
                            {
                                richTextBox_Show.SelectionColor = Color.Black;
                                richTextBox_Show.AppendText("每帧耗时：" + time1_sum.ToString() + "\n");
                                richTextBox_Show.ScrollToCaret();
                            }));
                        }
                        frequency = (uint)Math.Round(((double)time1_sum) / f);
                        time1_sum = 0;
                    }
                }
            }
        }

        private void event_LMS111ReadSerialport(object sender, SerialDataReceivedEventArgs e)
        {
            int data1 = 0;
            byte data = 0;
            byte data_temp = 0;
            //LMS111变量
            int count1 = 0;
            int count2 = 0;
            int count3 = 0;
            bool flag_start_lms = false;
            bool flag_data = false;
            byte[] temp = new byte[8];

            if (serial_receive_on)
            {
            ReadData_LMS:
                data1 = LMS111_serial.ReadByte();
                data = (byte)data1;

                if (data == 0x02)
                {
                    flag_data = true;
                }
                if (flag_data)
                {
                    if (data1 == 0x03)
                    {
                        //传输结束
                        LMSdateTransform(); //数据转换

                        end4 = System.DateTime.Now;
                        time4 = end4 - start4;
                        start4 = end4;

                        for (int i = 0; i < LMS_NUM; i++) //缓冲
                        {
                            LMS_data_temp[i] = LMS_data[i];
                        }

                        LMS111_done = true; //正确退出

                    }
                    else
                    {
                        if ((data == 0x20) && (flag_start_lms == false))
                        {
                            count1++;
                        }
                        if (count1 >= 26)
                        {
                            flag_start_lms = true;
                        }

                        if ((count3 < LMS_NUM) && flag_start_lms)
                        {
                            if (data == 0x20)
                            {
                                switch (count2)
                                {
                                    case 1:
                                        LMS_data[count3] = temp[0];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 2:
                                        LMS_data[count3] = (temp[0] << 4) + temp[1];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 3:
                                        LMS_data[count3] = (temp[0] << 8) + (temp[1] << 4) + temp[2];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 4:
                                        LMS_data[count3] = (temp[0] << 12) + (temp[1] << 8) + (temp[2] << 4) + temp[3];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 5:
                                        LMS_data[count3] = (temp[0] << 16) + (temp[1] << 12) + (temp[2] << 8) + (temp[3] << 4) + temp[4];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    case 6:
                                        LMS_data[count3] = (temp[0] << 20) + (temp[1] << 16) + (temp[2] << 12) + (temp[3] << 8) + (temp[4] << 4) + temp[5];
                                        count3++;
                                        count2 = 0;
                                        break;

                                    default:

                                        break;
                                }
                            }
                            else
                            {
                                if ((data >= 0x30) && (data <= 0x39))
                                {
                                    temp[count2] = (byte)(data - 0x30);
                                }
                                else if ((data >= 0x41) && (data <= 0x46))
                                {
                                    temp[count2] = (byte)(data - 0x37);
                                }
                                count2++;
                            }
                        }
                        data_temp = data;
                        goto ReadData_LMS;
                    }
                }
            }
        }

        //未使用
        private void Video_tick(object sender, ElapsedEventArgs e)
        {
            if (video_continue)
            {
                frame = capture.QueryFrame();
                frame_count++;

                //视频播放结束
                if (frame_count > (video_info.frameCount-1))
                {
                    richTextBox_Show.Invoke(new EventHandler(delegate
                    {
                        richTextBox_Show.SelectionColor = Color.Red;
                        richTextBox_Show.AppendText("视频已到末尾！\n");
                        richTextBox_Show.ScrollToCaret();
                    }));
                    frame = frame_temp;
                    video_timer.Stop();
                }
                else
                    frame_temp = frame;  
            }
            else
            {
                frame = frame_temp;
            }

            if (cali_flag)
            {
                if (remap_flag)
                {
                    cali.InitUndistortMap(frame.Width, frame.Height, out map1, out map2);
                    remap_flag = false;
                }
                Image<Bgr, Byte> temp = frame.CopyBlank();
                CvInvoke.cvRemap(frame, temp, map1, map2, 0, new MCvScalar(0));
                frame = temp.Copy();
                frame_camshift = frame.Copy();
                detect_target = frame.Copy();
            }
            

            if (templatematch_flag)
            {
                match_frame = frame;
                templatematch_flag = false;
            }

            frame_rawshow = frame.Resize(0.5, Emgu.CV.CvEnum.INTER.CV_INTER_LINEAR);
            imageBox_rawvideo.Image = frame_rawshow;

            //显示消耗时间
            end1 = System.DateTime.Now;
            time1 = end1 - start1;
            if (time_flag)
            {
                richTextBox_Show.Invoke(new EventHandler(delegate
                {
                    richTextBox_Show.SelectionColor = Color.Black;
                    richTextBox_Show.AppendText("显示耗时：" + time1.Milliseconds.ToString() + "\nCamshift耗时：" + time2.Milliseconds.ToString() +
                                                "\n特征检测耗时：" + time3.Milliseconds.ToString() + "\nLMS111传输周期：" + time4.Milliseconds.ToString() + "\n");
                    richTextBox_Show.ScrollToCaret();
                }));
            }
            start1 = end1;

        }

        #endregion

        private void radioButton_RecordAuto_CheckedChanged(object sender, EventArgs e)
        {

        }

        private void radioButton_RecordManual_CheckedChanged(object sender, EventArgs e)
        {

        }
    }
}
